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Appendix B-Derivation of 
Requirements Tool Dynamics 



1.0 


INTRODUCTION 


One facet of robotic systems requirements analysis is the analysis of 
system dynamics. Dynamic analysis is necessary to ensure that the pro- 
posed system will be able to both produce the forces and/or torques re- 
quired for task execution and withstand the stresses and strains of op- 
eration. Typically, the dynamic equations of complex systems are time 
consuming to derive and must be modified and solved repeatedly as sys- 
tem design concepts change. However, in the case of manipulator sys- 
tems, the entire process of formulating and solving dynamic equations 
can be automated. 

This appendix describes the derivation of a set of dynamic equations 
that can be used recursively to calculate forces and torques acting at 
manipulator joints during operation. The equations are valid for any 
n-link manipulator system with any kind of joints connected in any se- 
quence. The derivation is based on a technique described in Orin, et 
al (Ref 1). “ 
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2.0 


INITIALIZATION 


This section sets up the tools that will be used in deriving the dynam- 
ic equations. Notation is defined and coordinate systems (as well as 
the transformations between them) are described. 

Consider an n-link manipulator. Define the base to be link 0 and the 
end effector, or tool, to be link n. Define. joint i to be the joint 
between link i-1 and link i. With each joint, associate a right-hand- 
ed, orthogonal, Cartesian coordinate system ;_let Xi denote the coor- 
dinate system associated with joint i. Let X.b denote a coordinate 
system associated with the base. 

The location and orientation of with respect to link i is fixed. 

For rotary joints, the origin of is placed_at the intersection of 
the joint axis and the link axis of link i. Xj. oriented by defin- 
ing the x axis to point towards the center of gravity (eg) of link i 
and the y axis to point along the_joint axis. The z-axis is defined 
uniquely by the requirement that Xi be right-handed and orthogonal. 

For roll and sliding joints, the origin of Xi is fixed only with re- 
spect to X^, with the x-axis pointing along the link axis, and the y- 
and z-axes defined to make Xj right-handed and orthogonal (Fig. B2-1). 

Matrix quantities will be denoted by capital letters; small letters 
with an underscore will indicate vectors. Hence, T is a matrix, while 
t_ is a vector. To distinguish magnitude and direction in a vector, the 
following convention is used: t_ = tjj t where t is the magnitude of t_ 

and u_ t is a unit vector in the same direction as t_. 


Vectors and matrices may have different representations when expressed 
in different coordinate systems. A left subscript will be used to de- 
note the coordinate system in which a quantity is expressed. Hence, 

£V denotes a vector expressed^in terms of Xj.5 jv^ denotes the same 
vector expressed in terms of Xj. When no left subscript is written, 
quantities are assumed to be in base coordinates (i.e., in terms of 



i/i-1 Joint Axis 
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Transformation matrices and displacement vectors accomplish transforma- 
tions between coordinate systems. The transformation matrices account 
for rotational differences between coordinate systems, while the vec- 
tors account for translational differences between origins. 

Define jP^ to be_the matrix that transforms a vector expressed in 
terms of system into terms of a rotated system X j • That is, 

[ B— 1 ] j P i : 2^ -*• X j 

or 

[B-2] j P i = jv 

Once again, the transformation matrices only account for rotational 
differences between coordinate systems. The result, jv_, can be in- 
terpreted as being a vector of equal length as and with the same 
orientation, but originating f rom Xj , rather than (Fig. B2-2). 



To perform some calculations, the transformation of inertia matrices 
from one coordinate system to another will be required. Transforma- 
tions of matrix quantities requires two matrix multiplications. The 
formula for transf orming_jthe inertia matrix of link i, I-j_ , from the 
system Xj. to t ^ ie system Xj is given by 

IB-31 J 1 ! ‘ J P i ‘A A'! 

As the approach to dynamics is developed, certain quantities will be 
needed in many of the equations. Instead of defining them as required, 
the following is a list of the vectors, matrices, etc, that will be 
used in this appendix: 

s-f = Linear velocity of X^ with respect to an inertial 
coordinate system; 

w-j = Angular velocity of Xi with respect to an inertial 
coordinate system; 

flj = Angular velocity of X^ with respect to X n *_i ; 
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= Wj[ = Angular acceleration of Xi with respect to an 
inertial coordinate system; 

£i ” = Linear acceleration of Xj_ with respect to an inertial 

coordinate system; 

h-f 3 j = Position vector of Xj with respect to X± y if i / j; 

Position vector of the eg of link i with respect to X n - , 

if i = j; 

1^ = Inertia matrix of link i with respect to the eg of link i; 
m^ = Mass of link i; 
f -j = Reaction force acting at X-, ; 
t^ = Reaction torque acting at X n - ; 
g = Gravitational acceleration. 


The use of a tilde 11 ‘* n over a vector will denote the 3x3 skew-symmetric 
matrix of a vector cross product. That is, if a_ -M and b =Pb 1 ] 
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3.0 


A DYNAMICS SOLUTION 


The derivation of the dynamic equations of an n-link manipulator system 
can now be described. 

The following application scenario is envisioned. A designer receives 
a set of requirements for a manipulator arm. He creates an initial de- 
sign, which he then subjects to the motions that would arise from a 
typical operation, thereby obtaining a position, velocity, and acceler- 
ation profile for each joint. Using these profiles, the reaction 
forces and torques acting on the system can be computed. 

To solve for forces and torques from the motion profiles, a recursive 
procedure can be used. First, working from X.b to 2in» t ^ ie veloci- 
ties and accelerations with respect to an inertial system will be cal- 
culated using the motion profiles as input. (The motion profiles are 
assumed to_contain information on relative values like the angular ve- 
locity of Xj_ with respect to Xi-1*) Once this has been accom- 
plished, the relations f_ = m a_ and _t = la + w x Iw can be used to 
solve for the forces and torques acting at the origin of each coordi- 
nate system. If the system joints are single degree of freedom, one of 
the resultant terms represents an actuator force or torque, and the 
other terms represent the dynamic reactions acting on the system due to 
the motion. 

To simplify notation, all quantities will be assumed to be in base co- 
ordinates, and the base coordinate system, will be assumed to be 

an inertial system. After the dynamic equations are derived, transfor- 
mations will be considered. 
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3.1 


COMPUTING INERTIAL MOTION 


[B-4] 

[B-5] 


[B-6] 

[B-7] 


The equations that will be derived are recu£sive — the values at X^ 
are calculated from knowledge of values at X^ ^ and from knowledge of 
the relative motion of with respect to X±-l* 


Figure B3-1 depicts the coordinate systems Xi-1 anc ^ separated by 
a single link. In (a), the joint is rotary; in (b), the joint is slid- 
ing. If the joints were held fi^xed, velocities of Xx could be calcu- 
lated by knowing velocities at Xj.-i anc * t * ie displacement between ori- 
gins. In particular, the angular velocity of would equal that of 
Xi-i> while die linear velocity would be a vector sum of the linear 
velocity at Xi-l anc * the angular velocity crossed with the displace- 
ment vector hi-i^i* 


w = w 
— 1 — i-1 


s , — s , . Wj i hj < . 

— l — i-1 — i-1 -^L-1,1 

If the joint between the coordinate systems was free, movement at the 
joint would contribute to the velocities of the system Xf* The rota- 
ry joint of (a) would add to angular velocity of X while the slid- 
ing joint of (b) would contribute to linear velocity. The angular ve- 
locity of the rotary joint is and the velocity of the sliding joint 
is 


Putting these terms into the equations [B-4] and [B-5] yields 


-i = ^i-i + -i 


-i ~ ^1-1 + ^i-l^L-1,1 + h i-l,i^h i _ 1 


Equations [B-6] and [B-7] can be used repeatedly to calculate the abso- 
lute velocities of each of the coordinate systems Xj. through X^, 
given the values for the system X^, and the relative velocities at 
the joints. 
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a) Rotary Joint 





Figure B3-1 Joint Coordinate Systems 




Formulas for the angular and linear accelerations of can be found 
from [B-6] and [B-7] using the definitions 

“ = and a = * 

^1 " dF [ Si> 

■ If [ 2 i-i + ^ 

■ If + If W 

■2 i-i + If Itl i 1 ^ + a i If [ S nj 

’Si-1 + “A,. + > 

x i 

CB-8] »!-%.!+ “ilin + “i-! % 


Si - 3F [ Si> 

= dF + Si-1 — i- 1 , ( + dt ^ h i-l,i* Sj, 1 

X“X y 1 

■ If tsi.ii + [»i.i] * ii-i.i + Si-i x If ^i-i.i 1 

+ lF‘lF< h i-l,i)Sh. , i 

X*” 1 y X 

= -i-1 + -i-1 x + x , . + 

d d?_ i-1>i 

-i-1 x h i-l,i dt [ ^h , + dt 2(h i-l,i ):y h , 

X -L $ X 1 1)1 

+ lF< h i-l,i> IfV , > 

X“1 y X 

= a. , + a. , x h. , + w. - x 4r(h. . .)u, + 

— i-1 — i-1 — i-l,i — i-1 dt x-1 ,x — h 


i-l,i 


^i-1 x h i-l,i ( ^-l . > + dtl^i-l.i^ . , 

1“ X y X X” 1)1 


+ dF (h i-i,i )( ^-i x -^h 


i-l.i 


a . = a . i + a. , h. . . + 2w. - (fi. . .y, ) 

— i — i-1 — i-1 — i-l,i —i-1 i-1 , x— h. 1 

X“ X y X 

+ » 1 -i<»i-iiii-i > i ) +ii i-i,i 3! h. , . 

x—x ) X 
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Formulas [B-6] through [B-9] can be used repeatedly to calculate the 
absolute velocities and accelerations of each of_ the coordinate systems 
through given the values for the system Xfc anc * t * ie rela- 
tive accelerations at each joint. 

If the end effector is holding a body, then a coordinate system can be 
attached to the body (call it Xn+i). Velocities and accelerations of 
the body can be calculated by applying formulas [B-6] through [B-9], 
using the values calculated for 



3.2 


COMPUTING REACTIONS 


Once velocities and accelerations have been calculated, the values for 
forces and moments can be obtained. 

Assume that the end effector is holding a body. Let f_ and denote the 
external force and torque acting on the body. Let Xn+1 denote the 
coordinate system located on the body. Then the resultant of all the 
forces and torques acting on the origin of Xn+1 are f° un d by using 
the equations below: 

[B - 10] 4+1 = + 4+l4+l,n+l + Wii+Ai+l.n+P - B) - i 

[ B -H] = I n+1 “ n+1 + w n+1 I n+1 w n+1 + 4+l,n+l m n+l 4i+l 

+ ~n+l4+l,n+l + ^+l^n+l4+l,n+P " 


where f denotes the vector from the origin of Xn+i to the 

point of application of _f . 

The forces and torques on each of the systems Xi can be found in a_ 
recursive manner, beginning with those at ^ and working down to X^ 
using equations similar to formulas [B— 6 ] to [B-9]. 

Assjume that the forces and torques acting on X*+ 1 are known and those 
at X^ are sought (Fig. B3-2). 



m ± & 


Figure B3-2 Force and Torque Diagram 
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[B-12] 

[B-13] 


The values £j_+i and t-y jj can be viewed as forces and torques 
applied to the system Xj_ at a point located by h^ Similarly, 

m i§. can be viewed as an applied force and m^a^ as *a reaction 
force (or inertial force) acting at a point located by The 

forces and torques transmitted to the origin of Xi are gi ven by 

-i = -i+l + m i^i + %-i,i + ^rL^i-i.P “ 

-i = -i+l + ^i,i+l ^i+1 + I i-i + + — i— i,i 

+ - s) + 


Equations [B-12] and [B-13] are the recursion formulas that can be used 
to calculate the reaction forces and torques at each joint from the 
tool back to the base. 

Consider f^ and ty as each being composed of three constituent vec- 
tors directed along the coordinate axes. That is, 


I" 

II 

i f ix 

= 

i f ix 

+ 

0 

+ 

0 


i f iy 

L± f i Z J 


0 

0 


i f iy 

0 


0 

_i f iz 

and 

. t . = 

.L 


,t. 

+ 

~0 

+ 

0 

i-i 

i ix 

.t . 

1 ly 

± t ±z_ 


i ix 
0 

0 


i t ±y 

0 

0 

i t iz 


If joint i has only a single degree of freedom, then one of the con- 
stituent vectors of fj. and ty represents the actuator force or 
torque. The other five constituent vectors represent reactions at the 
joint due to the manipulator motion and external effects. 
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[B-14] 

[B-15] 

[B-16] 

[B-17 ] 


[B-18 ] 
[B-19] 


INSERTING TRANSFORMATIONS ■ 


To simplify the notation, transformation matrices were not included in 
the derivations of Sections 3.1 and 3.2. However, in any application, 
transformation matrices must not be overlooked. 

In Section 2.0, lower left subscripts are used to denote the coordinate 
system a quantity is expressed in. In this case_, jP^ is used to 
represent the transformation matrix from to Xy The base coor- 
dinate system will be denoted by Xfc and will be assumed to be an in- 
ertial coordinate system. 


Equations [B-6] through [B-9] are the equations used to calculate in- 
ertial velocities and accelerations, given the relative velocities and 
accelerations between coordinate systems. These equations are executed 
recursively, and so quantities that are calculated at one step are used 
in the next step. By storing the results of transformations performed 
at each step, transformations need only be applied at succeeding steps 
to terms that have not already been operated on. Therefore, including 
transformations only when necessary, equations [B-6] through [B-9] 
become 

b^i = b^i-i + b p i [ i^i ] 

b^-i = b^i-1 + b^l-l^b P i-lh-l-i-l,i^ + b^-l^i-l.i^i-l-h^^ 

A " bVi + b'i^iVa, 1 + b£i-i ( b p i [ iMi ]) 


\A± b— i-1 + b^-l ( b P i-lh-l^i-l,i^ 


+ 2 b^i-l ( b P i-l ®i-l , # ± 


1) 


b^i- 1 v b^i- 1 V i- 1 1 i- 1-i- 1 , 1 




])) 


+ b P i-l [(h i-l,i ) i-l^h 


] 

i-l,i 


Equations [B-10] and [B-ll] for calculating the forces and torques act- 
ing on a body held by the tool become 

b^n+1 m n+l^b^n+l + b^n+1 b^n+1 ^n+l^n+1 ,n+l ^ b^ 

” b P n+l *-n+l--* 

b^n+1 ^b P n+l ^n+l P n+l b P n+l^ b^n+1 


b^n+1 ^b P n+l ^n+l P n+l b P n+l ^ ^b^n+1 


b P n+l ^n+Ai+d x m n+l^b^n+l + tAi+1 b P n+l ^n+l-^n+1 ,n+l ^ 
“ b^ b P n+l ^n+l^n+1 , ! f n+1--* " b P n+l*-n+l-^ • 


B-14 



and equations [B-12] and [B-13] become 
[B-20 ] b~L = b^-i+1 + m i ( b^i + t£i t£i,i " b^ 

[B-21 ] b% = b^i+1 + t£i,i+l b^-i+1 + b P i [ i I i b^i 1] b^ 

+ b^i,i m i ( b^i + b^i b^i.i " 

+ b^i ( b P i f i I i b P i ^b^i 

Equations [B-14] through [B-21] form the complete set of equations 
needed to calculate reaction forces and torques resulting from a given 
manipulator motion. 
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Appendix C-Derivation of 
Simulation Tool Dynamics 


App. C~Derivation of 
Simulation Tool Dynamics 



1.0 


INTRODUCTION 


Dynamic simulation of a robotic system requires derivation of the 
system equations of motion. The derivation can be both difficult and 
time-consuming. By constraining the system being considered to a 
specific class of systems, a general set of equations can be derived. 
The equations of motion for a given system within the class can then be 
obtained by specifying the values of the coefficients of the general 
set of equations. 

This appendix describes the derivation of the equations of motion for 
the class of manipulators consisting of rigid links interconnected by 
rotary joints. In addition, a technique is outlined for reducing the 
system of equations to eliminate constraint torques. The derivation of 
the equations and the technique used to reduce the system were both 
initially described in a paper by W. W. Hooker in 1974 (Ref 1). 
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2.0 


INITIALIZATION 


To carry out the derivation of the equations of motion, the notation 
and terminology to be used must be presented. 

The links of the manipulator will be referred to as bodies. The base 
of the manipulator will be designated body 0, and other bodies will be 
numbered consecutively from the base. Let P^ be a fixed reference 

point on body 0. Let P. be the point fixed at the center of the 

joint between body i-1 and body i; P_^ can be thought of as the point 

of contact of body i-1 and body i. 

The following definitions will be used: 


T. 

mH 


F. 

—l 

t,H 


m. 


= External torque (not acting through a joint) acting on body i; 

= Hinge torque acting on body i through joint i; 

= External force (not hinge force) acting on body i; 

= Hinge force acting on body i through joint i; 

= Mass of body i; 


m 




l 

i 

jk 


w. 

SI. 


H. 

—l 


4± 


£ o 

Y. 


= Total system mass (m = » 

= Vector from P_^ to the center of gravity (eg) of body i; 

i f i - vector from P. to P., 

J i 3 

i = j - vector from P_^ to eg of body i (=R^ ) ; 

= Inertia matrix of body i about its eg; 

= $ . - m. R, . R. . ; 

x x j i kx 

= Angular velocity of body i with respect to an inertial system; 

- Relative angular velocity of body i with respect to body i-1; 

= w. - the angular momentum of body i; 
x — x 

0 if i = 0, 


w 


j x ft. + otherwise; 

" 1 1 i-1 


= w.x(w.xh. . ) + ^.xh. . + Z w x(w xh , . ) + i|> xh , , ; 

— i ■ — x —xx —x —xx n=0 — n -n -ti,n+l ^ — n,n+l 

= Vector from inertial origin to P^; 


= Vector from P^ to eg of body i. 


03 



The use of a tilde l, ~" will denote the 3x3 skew-symmetric matrix of a 
vector cross product. That is. 


if a = 

a l 

9 

and 

b = 


9 

then 


a 2 




b 2 




Y 




_ b 3_ 



a b = a 

x b : 


'0 

-a. 

a 0 ' 


"V 

— — — 

— 



3 

2 





a 3 

0 

" a l 


b 2 




-2 

a l 

0 


b 3 


The definitions above correspond in most cases to those in Hooker (1974). 
Figures C2-1 and C2-2 illustrate some of the definitions. 
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Inertial Coordinate System 



DERIVATION OF SYSTEM (JOINT 0) EQUATION 


3.0 

3.1 ROTATION ABOUT P 0 

We will start by summing the external torques acting on each of the 
bodies and equating this to the sum of moments about P G of each 
body. The equation is particularly easy to obtain if each body is 
considered separately. 

3.1.1 Body 0 about P q 

The external torques acting on Body 0 about Pq are: Tq, the 

external torque; and _Fq, the moment about Pq created by the 
external force. 

The moment about Pq of body 0 has three components: 

nirR p - the moment of in times the acceleration of P with 

0 ocro 0 u 

respect to the inertial system; 

2) ~ moment of times the acceleration of the eg 

of body 0 with respect to Pq; 

dt^Q-Q^ ~ t * ie change in angular momentum of body 0 

Therefore we have 

Vo*> + "oVo + dt<w ■ + Vo 

The equation [C-l] is useless unless it contains the quantities known 
or desired. That is, we need to rewrite equation [C-l] in terms of 
link lengths, joint velocities, and accelerations. 

By definition, an expression for Yq can be given as 

Y = h 
-0 “00 


[C-l] 


so 


Y = -t— (h ) = w x h 
-0 dt v -oo -0 “00 

and 

io - dt fa 0 xi 00 > * ^o xi oo + 2 o x(h o x!i oo ) 

because all links are rigid and all joints are constrained to rotate 
only. Furthermore, an expression for the angular velocity is given by 

W Q = fl 0 , hence w Q = ^ = 0) 
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That being the case, the acceleration can be written as 


t C— 2 ] 


[C-3] 


[ C— 4 ] 
3.1.2 


VV!!oo^o xl! oo + 2 o x (2 o x V 

where the last terms may be expressed as vq, i.e«, 

x -oo + 2 o x (2 o xl W = 2 o 

In addition, the first term on the right-hand side of the previous 
equation is 

x -oo = ^oo^o since -00 = -oo 

Combining the above equivalent expressions produces 
mR Y = -m R R ft + m R^ v 

0 oo-o 0 00 oo-o 0 oo-o 

recognizing that 

4— ($ w ) = $ w + w x ($ w ) 

dt v (TO (TO -0 (TO 

and using Wq = ^ + iJJq, and <5 qWq = Produces 

w) = $fi + $ £ +WXH 
dt v 00 0"0 0 X 0 0 0 

Substituting equations [C-2] and [C-3] into [C-l] yields 

m 0 & 0(A) “ m oVoJk) + m o*oo^o + *A + Vo + ^) x ^O = ^0 + * 0 (&) 

Combining terms using the identity = $• - m_R nn R nn and moving terms 

to the right-hand side yields UU 0 0 00 00 

m R q + 0 = T + R (F — m v) — w xH — $ ib 

0 00 0 00-0 -0 00 V -0 oV -0 -0 0^0 

Body 1 about Pq 


The external torques acting on body 1 with respect to Pq are: 

1) = External torque on body 1; 

2) Rq^F^ = foment about Pq of the mass times the acceleration 
of Pq with respect to the inertial origin. 
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The moment contribution due to the mass times acceleration of body 1 
about Pq again can be expressed in three terms: 

1) m^oi^o = foment about P^ of the mass m^ times the acceleration 
of Pq with respect to the inertial origin; 

2) m i^01— 1 = ^ oment: a fr° ut Pq °f m 2 ti mes the acceleration of 

the eg of body 1 with respect to P^; 

3) = Change in angular momentum of body 1. 

Again, as in 3.1.1, the moments and torques are set equal to each 
other. 



And, as in Subsection 3.1.1, the equation above must be expressed in 
terms of link lengths and joint velocities. 


By definition, the position is given by 

II " iiOl + jlll 
differentiation yields 
il = w 0 x h 01 + w x x h u 


and 

ii - i 0 * -01 + B o * <2 o x &oi> + *i x -11 + B i x <B i x SiP 

Inertial angular velocity is given by 

So “ s o 
Hi ■ .So + -1 


hence, angular acceleration is given by 

*o = -^o + *0 
— 1 = dt ^-o + -p 

= Qq + ^ + Sq x £2^ 

= So + % + -^1 

° * • 

Substituting in for and an< ^ rearranging terms yields 

Ii “ B 0 x ioi + io x —oi + io x ^11 + ii x lin + !•! x k u 
+ So x % + — Op + -1 x (w L ^ h U ) 

Combining terms containing yields the expression 


*1 


= v ( — oi + ^iP + 4 x + x 

+ w 1 X (w x X h n ) + jtj X hjj 


<5*0 x — 01> + ^0 x ^01 


By returning to the definitions, the terms on the right-hand side of 
this equation can be given as 

Hq x ^01 "** ^11^ = % x — ^ “ ” ^il^I’ and 

w Q x (w Q X h 01 ) + io X h 01 + Wj X (w 1 X h u ) + X h u = Vj 

~ »• 

Making the substitutions given above, m lR ni Yl can be written as 



Substituting the values of Wq and into ) yields 

d • dt 

x $ oHl 

or 

[ C— 7 ] ^ (*!»!> " + *iii +Hi x lii 

Substituting equations [C- 6 ] and [C-7] into equation [C-5] produces 

m 1^0l£o - m 1^01*C>A) - m 1^01^11% + m 1^01“l + + $ 1% 

+ 9 fa + Wj xSj = T x + R 01 F 1 

Use of the Identities *‘ Q - *, - end ^ end 

rearrangement of terms yields: 


[C- 8 ] m 1 R 0lfi() + *Jofl 0 + *oi% “ X 1 + ^01^1 " m I*l> " -Si x % - 9 fa 
3.1.3 Body 2 about Pq 

The external torques on body 2 with respect to Pq are: 

1) T _2 = External torque on body 2; 

2 ) **- 02—2 = Moment about Pq of the external force acting on body 2 . 

The moment contribution due to mass times acceleration of body 2 about 
Pq can be expressed in three terms: 


1) m R 


= Moment about P rt of m 0 times the acceleration of Pq with 


2(32^0 — i,wul ^ il ‘ 4U - q 

respect to the inertial origin; 

2 ) m 2 ^ 02— 2 = Moment about Pq of m^ times the acceleration of the eg 
of body 2 with respect to Pq; 

3) 2^ = ®k an 8 e *- n an S u l ar momentum of body 2 . 

Equating the sums of external torques to the moments of m about P n yields 

d_ 2 U 

t c ra 2^02^0 + m 2^02-2 + dt ^2~2^ -2 + ^02—2 

Once again, equation fC-9] must be rewritten to be useful. The equation 
must be put in terms of link parameters. 

As in the expression for body 1, the position of body 2 is given as 


-2 ^1 + -12 + -22 
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Differentiating this equation yields the velocity 

-2 = -0 x £oi + -1 x ^ 12 + 2 2 x il 2 2 

and the acceleration 

-2 = ^0 X -01 + ^0 X ^0 X ^ 01 } + ^1 x —12 + Zi x ( H X x h 12 ) 

+ w 2 x h 22 + w 2 x (w 2 x _h 22 ) 

Using the definitions: 

-o = -0 ’ 

— 1 = -o + — 1’ 

w. 2 = £ 0 + £ 1 + £ 2 , 

the angular acceleration can be written as: 

H 0 = % + 

% = i 0 + 

Wz = + Ai + ^2 + il + — 1 x — 2 

* ]]q + £2^ + ^2 i 2 

where ^ + 5*i x —2 = —2 

Making the above substitutions into the expression for and 
rearranging terms yields: 

^2 — ^ (hQi b .12 — 22 ^ ^1 ^ (hi 2 ^ ~ 22 ^ —2 ^ ^^22^ 

+ Wq X (h 0 X h Q1 ) +i 0 X h Q1 + w L X (Wj X Ji 12 ) + ^ x h 12 
+ w 2 x (w 2 x h^) + i 2 x hg 2 


-2 R 02-0 “ R 12 -l R 22^2 + —2 
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[C-10] 


[C-ll] 


[C-12] 

3.1.4 


[C-13] 


[C-14] 


Therefore, the second term on the left of equation [C-9] can be written 
as 


m 2 R 02^2 " - m 2 R 02 R 02^0 " m 2 R 02 R 12 % " m 2 R 02 R 22^2 + m 2 R 02-2 

The expression ^(^2—2^ can a ^ so siraplified as: 

IE ($ 2 - 2 ) = * 2*2 + *2 X $ 2*2 

• • • • 

Substituting in the expressions + anc * ^2—2 = ~2 

into this equation yields: 


^($ 2 w 2 ) = * 2 a 0 + $ 2 a L + * 2 ^ + * 2 i 2 + w 2 x H 2 

The expressions [C-10] and [C-ll] can now be substituted into equation 
[C-9] to produce 


m 2 R 02^0 " m 2 R 02 R 02^0 “ m 2 R 02 R 12 % m 2 R 02 R 22^2 + m 2 R 02-2 

+ * 2 O 0 + *4i + * 9^0 + * 9^9 + 2 , x H 2 = T 2 + R 02 F, 


2“1 


2“2 


02—2 


Rearranging terms and using the identity = $ 2 - m 2 ^ 02 \ 2 produces 

the expression 


m 2 R 02^0 + 


00-0 

Combining Equations 


»?-a. + ♦§,£, + * 2 ^ - t 2 + r 02 


(F 2 - m^) - w 


-2 x -2 


" *#2 


By carefully examining equations [C-4], [C- 8 ], and [C-12], a pattern 
for the equations of torques and moments about Pq can be identified. 
The general equation of body i is about Pq is easily seen to be: 


m.R 

1 


0 i £ 0 


+ Qnr&n + + 


0(P0 


01-1 


+ $ 



i 


li + R 0 idi " m i ^ 

-% x - *!&! 


For a system of N4-1 bodies, the equation [C-13] can be summed for all 
bodies i (i = 0,1,2,...,N) to obtain the equation of motion for all 
bodies with respect to Pq. In the interest of simplifying notation, 
define 


v N * n 

= E. <l> . 

Oi n=i Oi 


Then the system equation can be written as: 


N 


iloKVSo 1 + 4 oo 2 0 + *01% + ' 

+ T N + R 00 (|^ - "(jV 0 ) + . . 
~ Ej x - w N x 


4 o A ■ A + T -1 


+ • • . 


+ R 0N^N " m N%- ) “ -0 x -0 
*0^0 ' • • • ■ *1*1 
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4.0 


DERIVATION OF JOINT 1 EQUATION 


4.1 

4.1.1 


[C-15 ] 

[C-16 ] 
[ C— 17 ] 


The next step in deriving the complete set of equations of motion is to 
work outward from Pq and formulate an equation for each joint. In 
formulating the equation of motion for joint 1, the moments and torques 
are considered only for bodies beyond that joint. The hinge torque 
T? must then be considered as an external torque. One way of 
looking at this is to say that represents the angular effect of 
bodies 0 through 1 to the reaction at P^. 

ROTATION ABOUT P x 

Body 1 about P-| 


The 

external torques acting on body 1 about 
H 

Pi are : 

1) 

h 


2) 



3) 

R^F^ = foment of the external force 

about P^ 


The moment due to mass times acceleration of body 1 about P^ can be 
expressed as three terms: 

1) = Moment of about P^; 

2) m^R^Y^ = Moment of m^Y^ a bout P^; 

3) ^(* w ) = Change in angular momentum of body 1. 


The moments and torques above can be equated, yielding: 


m 


H 


i R u £ 0 + ’i R iA + i-< W " li + li + «i* 

dt 


As was the case in Section 3.0, the equation [C-15] must be expressed 
in terms of the link properties and relative joint velocities. 
Fortunately, much of the required algebraic manipulations were 
performed in the preceding section. Thus, we have 


“ - R oA> - 


R iA + a-i 


Substituting this into yields 

“A A = -m i^n^oi^o “ + m i^n-i 

As in Subsection 3.1.2, can be rewritten as 

= *i£ 0 + *1% + $ litl + Hi x Si 
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[C-18] 

4.1.2 


[C-19 ] 


Substitution of equations [C-17J and [C-16] in equation [C-15] produces 

m i R n £ 0 ” m i R i 1^01^0 " m i R n R n^i + m i R ii-i + $ i^o + 

+ $1^1 + Wj x Hf = + T j + RjjZi 

If terms are rearranged and the identity $£.. = is used, 

the result is 


m i R iip 0 


♦}<&> 


‘ilSi ' 


TJ 

T + T 
-i -i 


R n (E i - 


"lV - Wj 


x « 


- 


Body 2 about Pi 

The external torques acting on body 2 with respect to are: 


1) T2 


2 ) R 12 F 2 


The moments of body 2 about Pj are: 

1) i“2 R 12£o 

2) "2*12^2 

3 ) d ($ 0 w 0 ) 
dt 

Equating the external torques to the moment due to the mass times 
acceleration yields 

m 2 Ri 2 ^o + m 2 R 12~2 + = 1 2 + * 12*2 

dt 

Note that T^ is not being considered. This is because if we 
included it here, then a -T^ would have to be included in the body 
1 equation. The two torque values would then cancel when the equations 
were combined. 


As before, equation [C-19] must be rearranged. From Section 3«0, we 
have 


-2 " R 02-0 R 12 -l “ R 22-2 + -2 

Substitution of this equation into m 2^12— 2 y -*- e ^ s 1 



Furthermore, from Section 3.0, 


[C-21] 


[C-22] 


4.1.3 


[C-23 ] 


[C— 24] 


^($ 2 H 2 ) - * 2 S 0 + ^ 2 % + * 2 &2 + * 2^2 + *2 X ~2 

[C-21] can be substituted with equation [C-20] into equation [C-19] 
giving 


m 2 R 12^-0 " m 2 R 12 R 02^0 " m 2 R 12 R 12^-l " m 2 R l 2 R 22-2 + m 2 R 12-2 
+ $> 2 g 0 + $2^ + + $ 2^2 + x Ji 2 = -2 + R 12~2 


simplifies the equation to: 


m 2 R l 2—0 + $ 10-0 + $ 11-1 + ^ 12—2 “ -2 + R 12^-2 m 2 - 2 ^ 


k 2 A » *2 
T 11 

- w 2 x H 2 - ^ 


Combining Equations 

By examining the pattern of equations in Section 3.0 and by closely 
examining equations [C-18] and [C-22], we can see a pattern. The 
equation of motion for body i about can be easily written down: 


m l R li^0 + *10-0 + *11-1 + 


+ = 1 ± + R iiCEi - m i!i> 


-w 2 X H 2 - $ 2^2 

The equations of motion about P 1 for all bodies beyond P 1 (that is, for 
i = 1,2,... ,N) can now be summed to obtain the joint 1 equation. To 
simplify notation, let 

$?. = Z N <J>” 5 k = max ( 1 , i) 
ii n=k lx 

Then the joint 1 equation can be written as: 


i=i [m i R ii£o ] + *10—0 + °ii% + * • 

= Ij 11 + Tj + T 2 + . . . + T n 


+ 


+ (Sli m i^P + . • • + R 1n (£ n “n-n^ 


- x - 


" -N X % 


" *1*1 - * • * - $ N*N 
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5.0 


ADDITIONAL JOINT EQUATIONS 


From the experience gained in following Sections 2.0 and 3.0, it is 
apparent that the equations of motion for a regular system of bodies 
like a manipulator arm- have a pattern associated with them. One need 
only derive equations step by step until the pattern becomes clear, and 
then write out the rest by induction. Examination of the complete 
joint equations [014] and [024] reveal a distinct pattern in the 
joint equations. Using the convention 

$ ij = n5 k $ ij » k = 

the equation of motion about joint i can be written: 


[C-25] 



+ $ i(A) + *il-l + • • • 

- li" + 1 ± + I i+1 + . . . + T„ 

+ R^CF^ “ “-[Yp + • • • + R 


- «i x Si - • • • - Hn x % 

- - ... - 


+ 

iN ( -N " Vn> 
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6.0 


THE SYSTEM OF EQUATIONS 


[ C— 26 ] 


[C-27] 


The set of equations of motion for each joint can now be expressed 
together in matrix form: 


- 

m R n 

n=0 n On 

00 

•Si 

y N 5 

E, m R, 

n=l n In 
• 

v 10 

• 

• 

• 

. ^ • 

n=N- 1 m n^N- 1 , n 

• 

• 

<t>E 

N-1,0 

• 

• 

E N m ilL 
n=N n Nn 

&Z 

N,0 

N1 


— 


— « 

. . . ON 


s° 



a? 

• 

• • • *HH . 


• 

> . 


H 

1 

H 


H 


‘N 


N 


A + ^nnCL. “ m r,Y«) “ W X H - $ ill 1 

n— u n Un — n n— n “'ll — n n— n J 


N 


n=l fin + R in^ F r, “ v _ ) - W X H - $ ijj ] 
n-i n in -n n-n — n — n n-n 


N 


n-N fin + Win " m r, V rJ " w x 3 - 0 ^ ] 

n-iN — n iNn ~n n— n — n n n-n 


The matrix equation [C-26] contains N+l equations in N+2 unknowns. 
Several ways exist to make the system square. An additional equation 
may be derived, or an unknown may be dropped. The latter alternative 
is best suited for ROBSIM dynamics. For all the situations to be 
considered during the initial ROBSIM effort, the manipulators will have 
fixed bases, so Pq = 0. Hence, the first unknown is unnecessary, and 
we are left with 




^0 


0 





Si 


T H 



$ 



= 

“1 

| • 
• 

+ 

E(w) 




i 

t ’h 

% 
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7.0 


SOLVING THE SYSTEM OF EQUATIONS 


[C-28] 


[C-29] 


[C-30 ] 


[C-31] 


To simplify notation further, denote the system [C-27] as 
AQ = T + E 

Vector quantities were # used in deriving this^ system. Therefore, the 
angular acceleration, , is given as = [ftt, ft^jwhere each 

of the components of ^ is a 3x1 vector. Therefore, Q ,T, and E each 
are composed of N+l quantities, each of which is a three-vector, so 
each has 3N+3 scalar components. Thus, A is 3N+3 x 3N+3. For N = 7, 
solving the system would require inverting a 24x24 matrix. Such a 
matrix inversion is acceptable. 

We have been tacitly assuming that only the fl are unknown and all the 
quantities on the right hand side of equation C-28 are known. Consider 
the vector T, and examine the component Tg of T. By definition, 
jg represents the torque on body i acting through the joint i. 

For single degree of freedom joints, one component of Xi 
represents the known actuator torque at joint i. The other two 
components are unknown constraint torques. In order to solve the 
system, the unknown constraint torques must be eliminated and the size 
of the system reduced. 

The way to eliminate constraint equations is by projecting the system 
onto the unconstrained axes. For example, suppose we are solving for 
the motion of a single link around a joint. Then we have a system of 
three scalar equations in three unknowns, which can be represented as: 

Ax = t_ H- e_ 

where A is 3x3 and x _ , t_, and e_ are 3x1. 

Suppose our single joint has only a single degree of freedom - rotation 
around some axis. Let g_ denote the free axis of rotation. Then the 
effects of reaction forces and torques on motion around g can be 
computed by projecting all the effects onto the g axis. Since the 
vector dot product is a projection operator, the projection can be done 
by taking the dot product of the unit vector along g, g T , with the 
equation [C-29]. Writing x n and t n to represent the free 
components of x and t, the resultant scalar equation can be written: 

(g ,C Ag») x n = t n + g^e 

The equation [C-30] is then integrated to obtain a solution. 

To solve equation [C-28], let gj^, g 2 ,...,gjg denote unit vectors 
along the axis of rotation of each joint. Then, to reduce equation 
[C-28] to solvable size, the projections of the system onto the gj/s 
must be obtained. The resultant system is: 

(G^G) Q = T + G C E 
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& 2 0 


l 1 -, and Q , T are the components 


>f and T along the free axis. 
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8.0 


LINEARIZED DYNAMICS OF N-LINK SYSTEM 


[C— 32 ] 


[C-33J 


The governing equations of an N-link manipulator system are given by 


00 01 


01 11 


A IN A IN 
<5> $> 

NO 


0 


$ 


IN 

ON 

IN 

IN 

NN 


^0 




, Cl 

L -N 


2 0 


H 


H 


II 


T H 

L "N J 

N 


^ n [T + R (F - mv)-w xH - $ il; ] 

n=0 — n on -n n-n' -n -n n-^n J 

N 

y[T +R (F - m v ) - w xH - $ tL ] 

-n on — n n~n' -n — n n-n J 

n— I 


2 [T + R (F - m v ) - w x H - $ i/j ] 
~ n on “ n n-n' — n — n n-^n 

where the specific parameters have been defined in Section 2.0. 

Equation [C-32] can be written in matrix form as 

M($„) x = T + £(£, _f , v, w, $) 


where 


M = 


x = 


00 # 


NO 


[ S 0 


. .$ 


IN 

ON 


NN 


• V 


T = 


- T o 


H 


T 

— N 


+ 


N 

E n T 

n=0-n 


N n, 

2 A 

n=0 


and the vector, _g, is : a function of the position vector, p; force 

vector, _f; a vector, jv, which represents the moment due to 
centripetal-type force; the absolute angular velocity, w, and term, 
which corresponds to cross-coupling effects. 
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Equation [C-33] can be linearized by letting 


x = x* + 6x 

= g* + $£ 

v = v* + 6 v 
[C-34] w = w* + 6 w 

M = M* + <5M 

T = T* + <5T 


where the * terms correspond to nominal values of the respective terms 
and the 6 ( ) are perturbations about the nominal. Using equation 

[C-34], Equation [C-33] is given by 

[C-35] (M* + 6M) (|* + 6x) = T* + S T + £*+$£ 


Manipulating Equation [C-35] and keeping only the linear terms yields 
[C-36] M* 6x + Mx* = 6 T 4- 

This equation, in conjunction with Equation [C-34], represents a linear 
system equivalent to Equation [C-32]. Further clarification of the 
terms given in Equation [C-36] will now be given. 


Consider the second term on the left-hand side of Equation [C-36], i.e. , 


[C-37 ] 6Mx* = 6 


00 • 


'On 


$ N0 ‘ ’ $ NN 


^0 


— N 


It can be shown that the subinertia matrices are functions of the 
position vector, jd, i.e., 



where 


£' [—oo —oi • • • — 0N^ [£.1 £-2 • • • R ] 

Examination of Figure 03 further clarifies the definition of these 
position vectors. 



Figure 03 Position Vector, jp 


At this time, it will be convenient to change notation. That is, let 
the inertia matrix be given by 3x3 submatrices, 


M 11 M i2 - • • M 


In 


[038] <$M = 6 


M, 


21 


M 


n 1 


M 


nn 


where 


[C-39 ] M £ * Zi 
ij ij 


i = j = 0,1,.. . »N 
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Substituting Equation [C— 38 ] into Equation [C— 36 ] yields 


[C-40] SMx* = 


m ilk + 5M 12^2 + ' • • + 


6M n iXi + 6M n2 x 2 + 


. + a M x 
nn— n 


and examination of the variation of Mjj reveals that 


[C-41]SM. ,x* = <5 
iJ-J 


a n (r) 

a 12( £ ) 

a 13( £ ) 

a 2 1 (-E.) 

a 22 ( £ ) 

a 23 ^ 

a 3 i ( £) 

a 3 2 (R> 

a 33 (£) 


•• -k 
X . 

“J 


w 1J 


where use has been made of the fact that the subinertia matrices are 
functions of the position vector, p. 


Substitution of Equation [C-41] into [C-40] yields equation [C-42]. 
This equation may be manipulated into [C-43], where the subscript, _p, 
refers to the fact that the partial derivative of each element is taken 
with respect to the position vector, _p. For example, the notation, 

, means the partial of the ij component is taken with respect to _p. 

The result of this operation is a lx3n row vector that when multiplied 
by the 3n x 1 change in position vector, <$£, produces a lxl quantity* 
Also, it should be recognized that the definition of _x was substituted 
in its place — see previous definition of x. 


Equation [C-43] is further expanded to illustrate that [C-40] can be 
written as a product of a 3nx3n matrix, T_, and the 3nxl position 
vector, p, which produces a 3nxl vector. Specifically, this is given by 

6Mx = r_ 6£. 

To ease the coding of the previous equation, it is convenient to 
further decompose this equation. This can be done by partitioning the 
3nx3n matrix r and the perturbed position vector, 6£, into its 3x3 
submatrices and 3x1 position vectors, pi , pp , • • . , J> n > respectively. 
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[C-42] 


“ n (£) a i2^ a i3^ 


a 2 i^ a 22 (£) a 23 (£.) 


a 31^ a 32 (p) a 33 (^) 


a u (£) a 12 (£> a 13 (jj) 
a 2 i(£.) a 22 (£> «23^ 


“ 3 1<£> “ 32 <£> “ 33 <£> 
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0 

1 

-fN 

(jj 


6 MX = 


IVllE + Vl2j> 

' [“oAlj, + “ 0 y a 22 £ 
■ [“ox“ 31 £ + 1 \>y a 32 £ 


+ 

+ 

+ 


fi 

fi 

a 


Oz a i3£ 

Ct 

Oz 23£ 
Oz a 33£ 



. + 


/ 

r • 

fi d, , + fi d + fi a 

NX ll£ NY 12£ NZ 13£ ■ 

P 

| 6 £ ' 
In 



[fia fia fia 1 

fi£ 

► 

1 1 

NX 21p_ NY 22£ NZ 23£ J 

■ ^NX a 3l£ + ^NY a 32p + ^NZ a 33p ] 

In 

<$£ 

> 



In 



[^OX a il£ + ^OY a i2£ + %z a i3£ J 

3 

1-* 

o> 

\X) 

> 


i 

f 

[ fi NX « ll£ + ^ NY a 12 £ + ^NZ a i3£ ] 

f 

1 6 £ 

nm 

< 

! 

[%x a 2l£ + %Y a 22£ + %z a 23£ ] 

6p 

nl 

► +...+ * 


• %X Qt 2l£ + ^NY a 22£ + ^NZ a 23£ ] 

r 

6p > 

nm 

k 

[%X a 3l£ + ^OY a 32£ + %z a 33£ . 

cSp < 

nl 


k 

1 • • 1 

fia + fi a + fi d 

1 NX 3l£ NY 32£ NZ 33£ . 

| 6 p . 

nm 
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Performing this partitioning will produce the following result, 

* r. 


[C-45] 


r il r i2 


In 


nl 


nn 



6 P1 


6£ 2 


_ % 


where the 3x3 submatrices, r , are given by 


[ C -46]r kj 


n 

Z 

i=l 


“llx 

a 12y 

a 13z 


_ . * 

(i-l)x 

0 

0 

a 21x 

a 22y 

a 23z 


0 

.* 

Q a-l)y 

0 

a 

a 

a 


0 

o 

. * 

ft.. 

31x 

32y 

33z _ 

kj 

L 


(l-l)z_ 




Ra - 


.* 


a iiy 

a 12z 

a i3x 

J 

0 

n 

(i-l)x 

0 

a 21y 

a 22z 

a 23x 


0 

0 

• * 

ft/ . 
(i-Dy 

a 01 

31y 

a 32z 

a.. 

33x 

ij 

.* 

_ fi (i-l)z 

0 

0 

a llz 

“l2x 

“l3y 

~0 

0 

• £ T 

^(i-l)x 

a 21z 

a 22x 

a 23y 


(i-l)y 

0 

0 

a 

a 

a 


0 

.* 

ft , 

0 

31z 

32x 

33y J 

ij 


(i-l)z 
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[C-48] 


[C-49] 


[C-50] 


where the a matrices correspond to partial derivatives with respect to 
the x, y, and z components of the position vectors, pj. Equation 
[C-47] represents the desired form of the second term on the left-hand 
side of [C-36]. Because the first term of Equation [C-36] is 
self-explanatory, attention will not be turned toward the right-hand 
side of [C-36]. 

The change in torque, given by 5 Z, is the torque to be supplied by the 
motors which control each manipulator joint. Variations in the vector, 
j 5 , will now be examined. 

Since the vector is given to be a function of _p, Z> anc * Z> * ts 

variation is given by 

sg-fj + % + h s - + w 


where it is assumed that _f is a constant over the interval ;of interest 
and therefore does not effect the variation of g. 


Referring back to the specific definition of g_ from Equation [C-32], 


was shown that 


n 



n=0 


R n < F 
On “ti 


m v ) - w x H 
n“n ~n n 


$ y 

n~n 


it 


n 





R n (F 
On — r 


m v ) - w x H 
o — n n 


<S> y 

n~n 


|^®nl = 

Using Equation [C— 4 9 J , 


n 

v R (F - m v ) - w xH - $ y 
L on ~ n rr-n ~n n n~n 

n=n 

the variation in g can be written as 


<Sg 


11 


A _ 6 I R (F - m v ) - w x H - $ V 1 

= 1 I On n ~n~n ~ n n n n I 

n=0 


6j£ A 


n 


n=n 


i 


js , A - 6 R (F -m v )-w xH - $ ¥ 

6 nl = £ On — n — mr — n n n~ n 
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[C-51] 


[C-52] 


and because the terms inside the summation are identical, Sg_ may be 
obtained from examination of the variation in gn, i.e., 

6 g H A A + B + C 


where 

A A 6 |R (F - i v ) 1 
1_ On n n n _) 

B A 6 fw x H ~ 1 I 

= L _n ~nj 

C A 6 

Derivation of the A, B, and C contributions to the variation will now 
be discussed. Consider the expansion of A, i.e., 


A = SR (F 
On n 


* 

m v ) 

im 


m 


n 



5 v 

— n 
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[C-53] 


[054] 


or 


A = 6R n F * - m 6R v * - m R* S v 
On n n Orr-n n On-n 


It can be shown that 


i i 


Vn'"<| 6 nlx', 6 nly! 6 nlz 


nx 


0 

f 


ny 


0 

0 

f 


nz 


+ 3„,„i e. 


i i 

nly J ^n2z j p n3x 


0 

0 

f 


nx 


nz 


0 

0 


+ 3_,_'3_ 0 „.3. 


i ( 

, 1 3 o i k o 

nlz | n2x j n3y 


0 0 


f 0 
ny 


nz 


0 

f 


ny 


nx 


0 

0 



where the 3x1 column vectors ,§ n ^ , — n 2 * an< * — n3 are defined using. 


R A 
On = 


On On On 
r ll r 12 r 13 


On On On 
t r T 

21 22 23 


On On On 
r 31 r 32 r 33 J 


Specifically, the _§ s are given by 


' k , On On On x 

All A < r ll r 21 r 31 > 

R «l, 0n r 0n r 0n 1 

— n2 = ( 12 22 32 ) 

o' a Cr 0% 0n r 0n ) 

— n3 = ( 13 23 33 ) 
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and 


[C-55] 


[C-56] 


3r 


On 


— nls ‘ 


11 


9r 21° n 9r 31° n 


3S 


3S 


3S 


) ; S A X, y, z. 


In other words, the _3_ s given in f C— 54 ] correspond to the partial 
derivatives of the column vectors of the 3x3 matrix with respect 
to the x, y, and z components of the position vector, j3 n . 


Similarly, the second term of Equation [C-53] is given by 

* ~ 


m SR. v = m 
n On n n 


i 


6 * 3 o * 3 ^ 

nix | p n2y \ p n3z 


nx 


0 

0 


0 


ny 


0 

0 


nz 


3 i 1 3 0 1 3 o 

nly j n2z \ n3x 


0 

0 


nz 


nx 


0 

0 


ny 


3 .. I 3 0 ■ 3 q 

nlz j n2x j n3y 


v 


ny 


0 


0 

0 


nz 


nx 


0 

0 


3gn 


Continuing on with the derivation of A, Equation [C-53], the last term 
will now come under consideration. Using the definition of v^ from 
Section 2.0, this term can be written as 


m R. 5v = m R 5 
n On n n On 


n-1 


j-o 


w. x(w. x h , . , - ) + 

-j -j -j »j+i 


¥, X h. ... 1 

-J 3 ,1+1 J 


+ w x(w xh ) + ¥ xh 
— n — n -nn — n — nn 


Propagating the variational operator through the summation yields 
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[ C— 57 ] 


[C-58 ] 


C C— 59 ] 


m R. <5v = m R. 

n On — n n On 


n 1 

Z 5w. x (w. 

3=0 




+ ~3 x <S2 3 x -J,3+1 ) + £ 3 * % X %,3+1 ) 

+ 5 -j x -j ,3+1 + -3 * 5 -j ,j+i] 

+ 6w x(w xh )+w x (6w x h ) 

— n -n -nn — n ~n — nn - — » 


+ w x(w x6h ) + W xh + ¥ xSh 
— n -n -nn -n -nn -n -nn 


Examination of this expression indicates that by considering the term 
in the summation, the treatment of terms outside the summation will 
follow the same manipulative treatment. Therefore, consider the term 
internal to the summation, i.e., let 


m R 
n On 


where 

a A Z 
“ n_ j 


b A E 
-n A 


n-l 

E 

j=0 


Sw x h ) 
“J ~jj+l 


w. x (6w. x h. . ,,) 
“J ~3 “J ,J+1 


= a + b + c + d + e 


c A E w . x (w . x 6h , 

~ ' ” “J “J J ,J+1 


d A E xh, fjL1 

“J “J ,3+1 


e A T, x 6h. 

~n = . ~3 ~3 ,J+1 


The first two expressions, a_ and _b, can be written in terms of the 3x1 

vector, ij . , and the variation in w.j as follows: 

3 ~ J 


a = 


n-l 

E 

j=0 


0 

-n 

n 


‘31 ‘21 


31 


0 

"Hi 


21 "11 


'll 


j 


<$ W . 

JX 


) W , 


jy 


)w. 
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where 



To clarify the propagation of the jc term, consideration of a set of 
coordinate frames and definition of the term become a 

necessity. Therefore, consider Figure C8-lto help clarify the 
definition of ilj,j+l* 
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This figure defines as the vector from the origin of the jth 

link to the origin of the j+1 link. Because we have inferred the 
definition of coordinate frames in the definition, these frames will 
now be discussed. 

Because the equations of motion [C-32] are written with respect to an 
inertial coordinate frame, this is a natural starting point. Let an » 
inertial coordinate frame be defined by an origin at I; axes x, y, and 
z; and unit vectors t 9 j, and k. 

Consider body frames attached to the respective links at the joints. 
Referring to Figure C8-1, let the origin of the first link be defined as 
A; axes as xj, y^, and z^, and unit vectors, ej, e2, e 3 . 

The coordinate frame of link z is defined by an^origin at B; its axes 
as X 2 y£> and 22 ; unit vectors as e^, e$ y and eg. 

Similarly, the coordinate frame of the jth link is defined by its 

§rigin at^J; its axes as xj, yj and zj; unit vectors as ej, 

e j+l and e j+2* These coordinate frames are summarized in Table C-l. 


Frame 

Origin 

Axes 


Inertial 

I 

X Y Z 

tn 

Link 1 

A 

X 1 y l Z 1 

•+ + + 
e l e 2 e 2 

Link 2 

B 

y 2 y 2 y 2 

e 4 e 5 e 6 

• 

• 

• 

» 

• 

• 

• 

• 

• 

• 

• 

• 

Link j 

J 

X j y J Z j 

e j 6 j+l e j+2 


Table C-l Definition of Coordinate Frames 


Returning to the discussion of the _c term of Equation [C-58], it is 
recognized that the variation of hj^j-KL must be expressed in the 
inertial reference frame. However, since this tern is conveniently 
defined in the jth link frame of reference, a transformation from the 
jth reference frame to the inertial frame of reference is required. 
This can be expressed as follows: 

[C-61] = T IJ h j> j+i 

X J 

where the Tjj transformation matrix takes components of h expressed 
in jth reference frame and expressed them in the inertiaT frame. 
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[C-64] V A 


cos V -sin ¥ 0 

0 0 

sin m cos *F 0 

0 0 


! 6 »4 


cos 0 0 sin 0 

0 0 


-sin 0 q 0 cos 0^ 


* 0 a 


0 cos c()q -sin c})^ 


0 sin (p 


0 


COS <j) 


0 


In a similar fashion, h^^2 can expressed in the inertial frame as 

-*• 

h. 


12 


= T h | 

T IB 


B 


where 


t IB = t IA t AB 
and 

where the matrices , 0j, and are defined as in Equation 

[C-64] with exception that these quantities correspond to yaw, pitch, 
and roll of the second link with respect to the first link. 


In a similar fashion hj ^ can be given as 


[C-66 ] h, 

1 j 


= T 


U 3,3+1 


J 


where 


T AT T 
IJ = IA AB 


T 

J-1,J 


Using Equations [C-66] and [C-67], the variation of 
hjij + i is given by 
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[068] 


[069] 


[070] 


^■jhhh 

•HHH 


HWHN 


j+1 

«J 

Then using Equation [066], equation [068] can be expressed as 
5 h. 


1 n- l 

I 

ki ‘“i ! 83T ii 

h j J+1 

0 

0 ~ 

= n 

I i=0 


0 

0 

0 


1 

l 

0 

0 

0 


+ l>i 


i ‘HI. ! V 


J 


0 

0 

0 


h j J+1 
0 

0 


0 

0 

0 


r i i i 

”0 

0 

h. ... 
3 > J+1 

| 'J 

6<f> 


LX! t 2* ± ! t 30 ± J 

0 

0 

0 


60. 

i 



0 

0 

0 

\J 

_ i_ 



where t±> t/?, and t$ are defined to be the column vectors that 
makeup the Tjj transformation matrix. Taking the partial of these 
vectors with respect to yaw, pitch, and roll is then used to form 
Equation [0-69]. 

The expression for c from Equation [C-58] can now be written as 


n-1 

c = Zj W. 6h. 

- pi J -J’ 


j+1 


037 



where 


[071] W. = (wj (w*) 

j j 


and 


[072 ] 


W 


w 

jz 

“W . 

jy 


- w 

jz 


w 


jx 


W 

jy 

- w 

jx 


(Note that 1 denotes transpose and * corresponds to nominal values.) 


The next step in the derivation is to expand the d_ term of Equation 
[058]. Recall that 


n-1 

d A I Sf.xh, . 
" = j=o "J 


and since 


= w x n 
"j-1 — j 

then 

n-1 

[073] d = Z (5w x £2 x h. + w x6£2 x h, ) 

' “ j=1 “J-l “J -J.J+l -3-1 — J -5,3+1 

It should be noted the Equation [C-73] is of the same form as a and b 
of Equation [C-58], Since that is the case, 
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n-1 

" 0 

£ 31 " e 21 


’ 5w (3-1) X 

[C— 74] d = E 

-e 31 

0 e n 


*0-1)7 

j=0 





e 21 

- £ 11 ° 





— 

J 





— 


p 


— 

* 

r* _ 

0 

w (j-i)z 

_w (j-Dy 


0 

h. ... 

J ,J+l»z 

-h. ... 
j »J+i»y 


6ft 

jx 

" W (j-l)z 

0 

w (j-Dx 


-h. ... 

J >J+l>z 

0 

h 

J,J+l»x 


6ft. 

jy 

W (j-Dy 

" W (j-Dx 

0 


~ h 

j »j+i»y 

-h. ... 

3 »J+l»x 

0 


6ft. 

3 Z 


where 


£,- A (0. h. ... ) - 

u = jy 3 , j+i.z 


(0. h. ... )' 


d, A (ft. h. ... ) - (ft. h ) 

21= jz j,j+1,x jx j,j+1,z 


e:L A (ft. h. ) - (ft. h. ) 

31= jx j,j+l,y jy j,j+1,x 


From the previous discussion the term 


e A If, x h. 

“J “J ,3+1 


can be written as 
[C-75]eAEP H Q. ^,j+l 
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where 


[ C— 7 6 ] P. A 

J-l = 


0 

W 

-W 


j-l,z 


- w 


j-i.y j-i»x 


w 


j-i,z j-i»y 


-W 


J-l.X 


and 


[ C— 77 ] Qj A 


-SI. 


jz 


a 


si 

o 


jz 


jy jx 


-fl 




jy 

i 

jx 


The term m R n 6v can now be summarized as 
n On — n 


[ C— 7 8 ] m R n , v 
n On <5 -ii 


= m £L f a + b + c + d + e I 

n On [ - - - - J 

+ m £L 6w x (w x h ) 
n On I — n — n — nn 


+ w X (S w X h ) + w X ( w X 
— n — n — nn — n — n 

+ d'F x h + ¥ x 6h 
-n — nn — n — nn 


6h ) 
— nn 
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[C-79] 


Recognizing that the terms in the last set of brackets can be 
represented similarly to those in this first set, Equation [C-78] can 
be written as: 


m R 6 v = m R 

n On n On 


Wo + » 14 O.J + »] 

+ 6 Hj_i + M 17 0 -1 »j 6^^ 

+ [M 19 (n,tH-l) + M 1>10 (n,n + 1) + M 112 (n)] «v 


+ M (n) 6h + M 1 

1,11 — nn 1,13 — n 


where 

a A Z 6_Wj 


b A Z M 6w. 
“ — 14 —J 


c A Z M._ 6h. 

- 15 ~j,j+l 


d A Z M <$w. . + M 6ft. 

-= 16 -j-1 17 ~j 


e A Z M 6h . 

-=* 18 ~2 , J+l 

and 


6 w x 

(w 

x h ) 


M . 6 

w 

— n 

-^n 

— nn 


19 ■ 

“n 

w x 

(6 w 

x h ) 




6w 


— n 

"nn 


1,10 

— n 

X 

(w. x 6h ) 

_ 

M, ,, 

6h 

— n 

'—n 

— nn 


1,11 

“nn 

x 

h 


= 

M. io 

5 w + 

~n 

~nn 



1,12 

*il 

V x 

6h 


_ 

M 1 1, 

6h 

— n 

"nn 



1,14 

— nn 
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The expression for B (see Equation [C-51]) can be obtained by noting 
that 

B A - 6 (w x H ) 

n' 



Using suitable definitions from Section 2.0, this can be written as 

[C-80] B A M 1 (n) Sw 
1 J = 1,15 — n 


where 



n 


Similarly, C can be written as 


C- A - 6 r* ^ 1 

L n -nj 

= _ $ 6Y 

n — n 

which can be expressed as 

[C-81] C = M. .(n) Sw , + M n _(n-l) 6C2 
1,16 — n-l 1,17 

where 


M A - $ 

1,16 — n 


-ft 


a 

L n y 


n 


nz 


-ft 


ny 


ft 


nx 


-ft 


nx 


M A - $ 
1,1/ n 


w , w , 

— n-lz — n-ly 


-w , 
—n-lz 


w , 

— n— lx 


-w ! „ -w . 0 

-n-ly — n-lx 
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In summary, the linearization of the right-hand side of Equation [C-36 ] 
can be characterized by the linearization of the 6gn term given in 
Equation [C-50]. Combining equations, the appropriate equations for 
the variation in g^i can be expressed as 

[C-82]6g u - e/(M 11 - M 12 ) Si,, - A 

+ -”„“0„ <M 19 + M l,10 + M l,12 ) - M l,15 

+ '*11,11 ®nn + [ M l,13 ‘ " 1 , 17 ] + "1,16 5 

where 



A A 



n-1 

Z 

j=0 



M 13 (j,j+1) + M 14 (j, 



6w. 

“J 


+ M 16 (j,j+1) <Sw j _ 1 + M 17 (j-l,j,j+l) 60 

Using this procedure it can be shown that the N-link case can be 
written in the general form as a linear 3nx3n matrix equation 

[C-83] M S + CO + K0 = T + D£ 

where 



M = 3nx3n Inertia Matrix 
C = 3nx3n Damping Type Matrix 
K = 3nx3n Stiffness Type Matrix 
T^ = Applied Joint Torques 
D = 3nx3n Position Modifier 
jd = 3nxl Position Vector 

The clarification of Equation [C-83] is easier to obtain using the two 
link case. This case is presented in the next section. 
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9.0 


LINEARIZATION OF THE TWO-LINK CASE 


[C-82] 


[ C-8 3 ] 


[C-84] 


[ C— 85 ] 


[C-86] 


The equations formulated for the N-link case will now be applied to the 
two-link configuration. For this case the equations of motion are 
given to be: 


+ *io> 4 + ii k - V + h + h 


+ R oo % - "oV 


R oi ( h - 


“iV - ^0 


x ^o 


- $ rlr, ~ 

0-0 


*1 


x H - $ V 


*}o % + ■ h* + *1 + *01 - "iX 


Si x Si - 4 A 


Using the definition of the inertia dyadics from Section 2.0, these 
equations can be written as: 


(4 0 - m 0 *00 R 00 + 4 1 - "l R 01 R 01> “o + (4 1 

■ 1 0 H - *0 - 2! + * 00 (F 0 - Vo , + R 01 ( £i - 
- Wq X ($ o Wq) - Wi X w : ) - $ 1 

w i ■ m i R u v 4) + (4 i - m i R u v 4 

= ll-li - R 01 

Theae equations are now of the form 
Mx = + _g_ 


m l R 01 R 11 




x £ i ) 



) % 
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where 


[C-87] 
[C— 88] 

[C-89] 

[C-90] 
[C— 91 ] 

[C-92] 
[C-93 ] 


r 


M 


i 0 0 00 00 


+ $ 1 - m 1 R Q1 R q1 


$ 1 m l R 11 R 01 


IJ 

J 0 + ^0 + ll 

ll H + Ij. 


& = 


$ 1 m i R 01 R 11 


$ 1 m l R 11 R 11 


R oo (z o - m oV + R 01 ( ^i • m &) 

- Hq x - Wj x (*^1) - % x %> 

R 01 <l x - ^ x x “i> 


and 

x = 

Following the discussion for the N-link case [C-86], can be written as 

fil-f'SR + 6 MR* = <$T 6g. 


where in this case the term involving the perturbed inertia matrix is 
partitioned into its 3x3 submatrices as 


f *; • JC 

Oi'iX = 


where 


r r 

11 ' 1 12 


r 1 r 

21 » *22 


6 J1 
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<$!> 4 R nrw R nrw R r 


2 I "00x “00y “OOz l R 01x R 01y R 01z 


] 


6 £i 


6 £o 
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Using this fact and Equation [C-90] allows the left-hand side of 
Equation fC-91] to be expressed as 


[C-94] ^ + <$Mx s 

_M 11 M 12 

* 

1 

i 

i 

r r 
11 12 


<S£i 


M 21 M 22 


5^ 

T 

r r 
^21 22 _ 


6£ 2 







w 


Manipulating the equations on the right-hand side of Equation [C-86] 
can be given as 




6g n 

6g 21 


where 


[C-95] 6g n = 6^R 00 - m^) + R Q1 - m^) - ^ x 

- x w^) - (Wq x J 


and 


[C-96 ] 6g 21 - 6 |r qi 0^ - m^) - Wj x ^ (oj q x 

Using the results from the N-link case it can be shown that 
[C-97 ] 6g 1:L = ^ + c 

where 


= TOO ( £o - m oV + 5 01 ( Ii - 


A, 6 Ir 

1- [l 

B 1 = _6 j^o x (^^p) + X (^wpj 

C 1 =“ 6 [ $ 1 x %)J 

Similarly the g£i element can be written as 

= A + B rt + C 
S 21 222 

where 
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A„ = 


2 = 


6 

-6 

-6 


[r 01 (F 1 - Vl )j 

I 


Wj X (* 


*1 % x %> 


Then, because (see N link development) 


[C-98] Sr F - 


•On 4 ■ ■„ SR 0„ 4 ■ - *‘ U M % 

the quantity can be written as 


A i = 


M ll (1) " M 12 (1) 


J <5% + £^(2) ” M i2 (2) 6£ 2 
{- m 0 R oo - "i R 01 s -i} 

Examination of the last term in brackets yields 

~ m 0 R oo 6 ^o ■ m i R 01 = - m o R oo [% x ^0 x 

+ Wq X Sv^ x h Q1 
+ Wq X Wq X 6h oi j 


~ m i R oi 


6wq x Wq x l^i 


+ Wq x 6 wq x h Q1 


+ x ^0 X 6 V 


+ x x _h^ 


+ w, x 6 wt x h 

“I “I 11 


+ X ». 


-1 x %11 ' 


Following the procedure given in the previous section, Ai can be 
written as 
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A l = 


M ll (1) - *12™ 


6 £i + 


M ll (2) " M 12 (2) 




-^cfa + b + c*| 
n=0 [-n -n -nj 


where 


a =m R .L 6.w. x ( v, x h ) 

n n On j=0 -J — j J >J +i 


b = m R n l 5v7. x (6w x h. ) 

n n Onjio j ~3 3*3+1 

c = m R .L vr. x (.w. x Sh J 

n n Qnj=0 -3 J J>J +1 


Following the description given in the N-link linearization, the terms 
above can be written as 
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n 21 
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n On 
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n ll 



- n 21 
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JZ 
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JX 
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jy 

“W. 

JX 
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<Sw 

jx 

6w. 

jy 
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jz 


0 h — h 

j,j+l,z j,j+l,y 

-h. 0 h 

J,j+l,z j,j+l,x 

h j,j+i,y h j,j+i,x 0 


6w. 


jx 

jy 
6 w. 

Jz 


c = m R Z TT p - 
-n n On . n W. 6h. 

J=0 j 


where Wj is defined in Equation [071]. 


048 



Expanding these equations yields 



£o + £l‘ m 0 R 00 <”0><»0>' S >!01 + ”l‘ i 01 <2 0 >< w 0 ) ' Sh 01 

+ (w 1 )(w 1 )'«h 11 

where all terms have been previously defined. 
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Summarizing up to this point, it has been shown that 


1 j 6 s n = a 1 + b 1+% 
and that 

h ' [ M ll (1) - M 12 (1> ] + [ M 11 (2) ' M 12 (2> ] % 

- [«o + i 0 + + ii + s 0 + =i ] 

or that 

tc-1001 a l - s u « El + s 12 S £ 2 - s 13 «» 0 - s 14 s 2l - - s 16 «h u 

where 

S 11 - - M 12«> 

5 12 A M n <2) - M 12 (2) 

5 13 A m 0 *00 [ <V + <V J + ”1 *01 [ <V + <V %!> ] 

5 14 A "! *oi [<Sl> + <Hl> ( Vj 

Continuing on, the manipulation Bj_ and Cj_ into a suitable form will 
now be conducted. Recall that 

B-l A - 6^ X ^ + Wj X ^wj 
and because 

—1 = “^ w o x ^0^0 " ~ 1 x ^i-l " — l x ^l^—l 

this can be written as 
[C-101] B x = M x 15 (0) 6WQ + M x (1) 6w x 


where 


/I 0 
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\ 

[ ! 

31 

21 

1 

M (n) A - # ( -v... 
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v 

+ fw I 1 

1,15 = \ ! 31 


11 

L^.J j 

xi v oi 

“V 

0 

J 

N ! 21 

11 
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and [w n ] is defined in Equation [C— 72] 
In this case, 


A - 6 ^(Wq x 


which can be manipulated into 

% = - X ^ + «0 X 6 ^i] 

Therefore, following the N-link derivation 
[C-102] ^ = M 1>16 (1) Swq + M 1j17 (0) 6^ 


where the M matrices have been previously defined. 

Now returning to equation [C-99] and substituting appropriately from 
[C-100], [C-101] and [C-102], the variation in gjj can be written as 



Attention will now be turned to the linearization of the g 2 i 
element. Because 

[C-104]<5g 21 A 6 - Wj. X - ^(Wq x 


and that 

i 2 ' S *01®1 ■ ”l-l ) - “l R 01 6 ^i 
■ S 12 6j>, - m, R„, ov, 


1 01 


then 



Now noting that 


6 (w^ x $wp = ^(1) Swj. 

S [*l ( Ho * %>] ’ H + M l,17 (0) S % 

Then the variation in g21 is given by 
[C-105]5g 21 = S 12 6p_ 2 - S u Swq - S 14 6w x - &h Q] - S 16 <5h n 

+ M 1j15 (1) 6w l + M 1j16 (1) 6w q + M 1>17 (0) 6% 


Equation [C-103] can then be written as 




At this point Equations [C-104] and [C-105] can be combined into the 
vector form 
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This equation can be placed into simpler notation as 



where the matrices, U, have obvious definitions. 

At this point additional definitions are used to further decompose 
[C-108]. For example, it can be shown that (see Equation [C-66] in 
section 8.0 on N-link linearization) 
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where the 3x3 matrix Toi comprises elements internal to the brackets 
and the 3x1 vector 66^ is given by 

A [«♦„ % «» 0 ] 

Similarly, it can be shown that 
[C-110]Sh 1;L + Tll (0) 60 q + T u (l) 6^ 


In addition, the fact that wq is equal to and that w^ equals the 
sum of and allows: 

6wq = 

[C-112] r 

6wj = SQq + 6^ 


Therefore, using Equations [C-109], [C-110], [C-lll] and [C-112] in 
[C-108] yields 



Where the fact that H is defined to be the first derivative of the 
relative angular position. This can be further compressed to yield 


5g 


<$j> 


<se 


6JL 

—11 

“ [ D n] 

1 

* frj 

-o 

• 


-o 



_ 6 \ 
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At this juncture the right and left-hand side of Equation [C-42] can be 
written as 



Combining terms, this equation is given as 



where 

D 4 [ u n]‘[ r ] 

This completes the linearization of the two-link case. 
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10.0 STATE-VARIABLE REPRESENTATION 


As discussed in the main body of this report, the simulation framework 
required for the manipulator dynamics is state variables. This being 
the case, this section will show how the linearized equations for the 
two-link case can be placed into the S.V. format. This will be used as 
a stepping stone to obtain the N-link representation. 


' Consider the linearized equations of the 2-link case, i.e. , 


M 11 

M 12 


% 

+ 

c n 

C 12 



+ 

K n 

K 12 


% 

_ M 21 

M 21_ 


A 


5l 

C 22_ 


A 


> 

K 22_ 




[C-116] 



6T, 


D 

D 


6 v 


-1 


11 

12 


*-1 

= 


+ 






V 

1 


_°21 

D 22_ 


& 

I 


where 



and the torque terms, an< ^ corresond to the torques 

supplied by the servo motors at the respective joints. Equation 
[C-116] can be rewritten in the form 

[C-117 ]Mx^+ Cx^ + Kx^t + Dr 


where M, C, and K are 6x6 inertia, damping, and stiffness type 
matrices; _t the applied torque vector; jr the 6x1 position vector; D the 
6x6 position modifier; and x the 6x1 angular position vector - three 
for each joint. 

Letting 

[C-118 ] = x 2 
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and substituting [C-118] into [C-117] yields a set of 12x1 first-order 
equations given by 

[C-119 ]My + Ky = a + Br 


where 




Equation [C-119] can now be discretized by solving the 12 first order 
equations 

[C-120]jr = A + E ju 


where 

A A M -1 K ; E A M _1 ^I bJ 
and the vector n defined as 
u' A_ |ja _r J 

The solution to [C— 120 ] is given by the well-known equation 
[C-121]^ (t) = z (t - o ) + / <j,( T ,t 0 ) E (T) u (T) dx 

fc o 
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Assuming 11 is constant over the time interval tfc to t^+i [C-121] 
can be written as 

[C-122] x (k+1) = <J>(k+l,k) x (k) + 0(k+l,k) u (k) 


where t, , , 

k+1 

0 (k+l,k) A / 4><T,t k ) E (t) dx 


The extension of the state-variable technique to the N-link case is 
clear once it is recognized that [C-117] for the N-link case becomes a 
3N set of second-order differential equations. Reducing these 
second-order equations into first order equivalents produces a set of 
6N equations. Because all that changes in going from a two-link case 
to a system represented by N-links is the dimensions Equation [C-122] 
is valid for the Nth link case. 
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Appendix D-Derivation of 
Requirements Tool Control 
Law 



1.0 


INTRODUCTION 


This appendix presents in detail one of the many methods by which a 
human being can interactively control an n-segment manipulator arm, in 
which segments are connected by rotational joints*, The method present- 
ed here is known as coordinated rate control, and was first introduced 
by D. E. Whitney in 1969 (Ref • 2). Since then, the method has been used by 
other workers and modified* This presentation explains the principles 
behind the method, as well as some of the details needed to implement 
it. The coordinated rate control algorithm derived here has been in- 
corporated into ROBSIM to drive the requirements analysis tool de- 
scribed in Chapter IV of the main volume. 

Coordinated rate control was developed to provide a method of manipula- 
tor control whereby an operator could input directly details of end-ef- 
fector control, rather than dealing with the end-effector indirectly, 
as when controlling individual joints. In coordinated rate control, 
the operator specifies the rate of end-effector motion (e.g., 2 in./s 
straight up) and the control algorithm transforms this command into 
rate commands to individual joints. The resultant of the joint motion 
is then the end-effector motion that was specified. 

The algorithm for calculating joint rates from end-effector rates can 
be summarized as follows. The movement of every joint contributes to 
the motion of the end effector. In fact, end-effector motion is the 
resultant of all the contributions made by the joints. By expressing 
end-effector velocity as the sum of joint contributions, a system of 
equations is obtained that relates end-effector rates to joint rates. 

The system of equations is linear, and can be solved by using tech- 
niques from matrix theory. 

Manipulator joints consist of two general types — rotary and sliding 
(Fig. Dl-1) . The development to follow considers arms having only 
rotary joints, but the logic behind the development is useful for ar- 
bitrary arms containing both types of joints. 

In addition to end-effector rate to joint rate conversions, this docu- 
ment also discusses some of the accuracy problems with which a control 
system must contend. Generally, the joint solution derived from a de- 
sired end-effector rate is valid only instantaneously. As the joints 
move, the relationships between links change, and the coefficients of 
the control equations are also altered. Physical limitations prohibit 
instantaneous re-calculation and solution of the control equations. 
Approximations can be made to simplify the system and speed up computa- 
tions, but the accuracy of the solution is reduced. Some tradeoffs be- 
tween speed and accuracy are discussed. 

Occasionally, the matrix defining the relationship between joint and 
end-effector velocities may be singular. Singularity would occur, for 
example, if a motion could be effected in more than one way, or if a 
motion was not possible. In Section D.2 a solution to this problem is 
presented . 
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Sliding; 

Figure Dl-1 Joint Types 
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2.0 


COORDINATE TRANSFORMATIONS 


To perform kinematic analysis of a manipulator system and develop con- 
trol equations, we must define a variety of coordinate systems and un- 
derstand the procedures involved in converting from one system to 
another. This section defines several coordinate systems and develops 
the transformations between these systems. An example is described. 

Let Xq denote the inertial reference frame that is fixed with respect 
to the manipulator base. Call Xq = (xq, yQ, zq) the set of 
world axes. Let X e = (x e , y e , z e ) denote the system of coor- 
dinate axes fixed with respect to the manipulator end effector. Call 
^ the end-effector axes. In addition to end-effector axes, define a 
set of axes with origins fixed at joint centers: 

5L = y_j,, z^) is the set of coordinate axes at joint i. 

All the coordinate systems are right-handed, orthogonal systems. In 
the case of joint systems, it is understood that the y axis points 
towards the terminal end of the manipulator along a line parallel to 
the terminal link associated with the joint. 


Figure D2-1 shows a four-joint manipulator and its coordinate systems. 
Note, however, that due to space availability, X 4 was left out of the 
figure. 



It is apparent (Fig. D2-1) that the coordinate systems differ from each 
other in two ways: their origins are displaced, and the axes may be 

rotated out of alignment. Therefore, if a vector is known with respect 
to one of the systems, the rotation and translation between systems may 
have to be considered to write the vector in terms of a second system. 
The transformation between systems is accomplished by using transforma- 
tion matrices and displacement vectors. 
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2.1 


TRANSFORMATION MATRICES 


An orthogonal matrix can describe the transformation between Cartesian 
coordinate systems that have been rotated with respect to each other. 
This orthogonality property is important, because it means that if P is 
an orthogonal matrix, then = P t . That is, the inverse trans- 
formation is just the transpose of the original matrix. 

The following is a development of the transformation matrix for a sim- 
ple, two-dimensional case (Fig. D2-2). 



Figure D2-2 

TbJo -Dimensional Coordinate 
Transformation 


The matrix for the transformation from system Xl to system Xo> w hen 
applied to a vector in coordinates, will yield the coordinates of 
the vector in the system. The matrix can be obtained by writing 
the coordinates of xj and y^ in terms of X() aru * Yq* That is, 
if qP^ is the matrix describing the transformation from to Xq , 
then 

o p i ■ [0*1 ; o*i 

where each of qxj and gy^ are unit 2-vectors in Xg. So, de- 
composing xj_, 21 In terms of xqj yg, we obtain 

-sin 9" 
cos 9 

Thus, 

sin 9 
cos 9 


0 P 1 = 


cos 9 
sin 9 


O 5 ! 


f cos 9 
sin 9 


D-4 



And, the reverse transformation, ^Pq, from Xo t0 Xi> is given by 
cos0 sin0 

-sin0 cos0 

Applying trigonometric identities will show that this approach is equivalent 
to the more familiar approach, in which the entries are the cosines of 
the angles between coordinate axes (Fig. D2-3). 




Figure D2-3 

Two-Dimensional Coordinate Transformation Matrix 


In three dimensions, the concept is easily generalized. 

To develop the transformations between coordinate systems on a manipu- 
lator arm, we do the following: 

1) Compute the transformations between adjacent coordinate systems 
with the manipulator in a reference position. Call this matrix 



2) Compute the transformation matrix for each joint in terms of the 
joint motion from reference position as a function of 0£j). Call 
this matrix jP^ . 

3) Then the complete transformation jP-^ is given by 

jP± = (jPiXjP?) 

Now, a vector given in any manipulator coordinate system can be trans- 
formed into any other manipulator system by repeated use of the jP-[ 
matrices. For example, suppose e v is a vector given in terms of the 
end-effector axes. (Denote the system a vector is expressed in by a 
left subscript.) To find qv apply the sequence of transformation 
matrices : 

0 V = 0 p l l p 2 • • • n-l p n n p e e^ 
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Once again, the inverse transformation would use the transpose of the 
matrices above. 

e¥ = (o p l • • • n p e) t 

= n p| . . . 0 p i o- 

= e p n • • • l p 0 0^ 


2.2 DISPLACEMENT VECTORS 

Transformation matrices only account for the rotation between coordi- 
nate systems, not for displacement between origins. Therefore, using 
the transformation 

jV = jPi iV 

yields a vector jv, of the same length and direction as ^v, but 
originating from the origin of Xj (Fig. D2-4). 






Figure D2-4 Coordinate System Displacement 


To obtain a vector representing distance and direction — e.g., from the 
origin of Xl to the tip of ^v — the displacement between the origins 
of different coordinate systems must be known. 

Let h^ j denote the vector from the origin of X± to the origin of 
Xj. T?ius h-^j = By applying the parallelogram law for 
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i 



vectors, the vector from the origin of Xj to the tip of ^v can be 
obtained by adding jv and , j * T ^ at ^ s > 

j— = j— j— j , i 


or 


j- j P ii- + j^j ,i 


Note that, to be added, vectors must be expressed in the same coordi- 
nate system. Using this notation, this means that the lower left sub- 
script of every term must be the same. 


Now, the use of transformation matrices and displacement vectors will 
be combined by means of an example. Figure D2-5 shows a three- joint 
manipulator. Let e d denote the location of an object in the end-ef- 
fector coordinate system. Then the location in world coordinates can 
be calculated by working backward through each coordinate system until 
the world axes are reached and gd is determined. 



Figure D2-5 Three-Joint Manipulator 


To X 3 : 3 d = 3 P 3 d + 3 h 3>e 

To X 2 : 2 i = 2 P 3 [3 p e ei + 3^3, e] + 2^2,3 

To Xx: id = iP 2 [ 2 P 3 [ 3 P e e d + 3 h 3je ] + ^ 2 , 3 ] + 1-1,2 

And, to X 0 : Q d = 0 p l[l p 2[2 p 3 [3 p e ei + 3^3, e] + 2^3, e] + llil , 2] + ok),l 

Simplify the notation by multiplying out the quantities in brackets and 
extending the notation by defining jPj_ = jPj +1 j+l^j +2 • • • i-l p i for 

Then, 

Ol = 0 p e ei + 0 P 3 3^3, e + 0 P 2 2^2,3 + 0 P 1 1^1,2 + 0^0,1 
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3.0 


CONTROL EQUATIONS 


In the preceding section, the notation and concepts needed to derive 
the control equations for the SOS manipulator were described. This 
section presents a derivation of those equations. 

The inputs to the control equations are end-effector rates. Any motion 
can be described by six rates: velocities in the x, £, z directions, 

and angular velocity around the x> z axes. Desired end-effector 
rates are input by the operator. The outputs are the individual joint 
velocities needed to effect the input motion. 

The derivation to follow is a means of resolving joint velocities to 
obtain a resultant end-effector velocity. Equating the resultant to 
the input yields a set of linear equations of the form 


D3-1 [Input] = J • [joint velocities], 

which may be solved by inverting J (in some sense) to obtain the joint 
rates. 

The goal in designing a human-operated, interactive control system for 
a manipulator arm is to minimize the variables the operator must con- 
trol. A simple control system can be designed so that the operator 
directly controls the rate of motion of each individual joint. How- 
ever, this is not a desirable system because to produce a specific mo- 
tion of the end effector, the operator must somehow anticipate the re- 
sultant of a set of joint motions. For one or two joints, this is not 
difficult, but for a six or seven-joint manipulator, the task is quite 
complex. For sensitive operations, this kind of control system is not 
practical. 

A better control system allows the operator to control only the end-ef- 
fector motion. A computer reads the input commands and performs the 
necessary calculations to produce the required velocities of each 
joint. Such a control system is practical for most any task. This is 
precisely the control system to be implemented. 


Consider angular rotation of a manipulator end effector. The rotation 
is about a single axis and has associated with it a velocity. Both the 
axis and the velocity comprise an angular velocity vector. Let w e 
denote the angular velocity of the end effector. Then w e can be de- 
composed into components about the x, y, and z axes of some coordinate 
system. So, in world axes, 


OHe = 


O^ex 

O^ey 

0-ez 
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D3-2 


D3-3 


D3-4 


Let Wj_ denote the angular velocity of joint i; that is, the angular 
velocity due to the motion of joint i alone. If the angular velocity 
of the first link is w^, then the angular velocity of the second link 
is the sum of w-^ and The angular velocity of the end effector 

is the sum of the angular velocities produced by each joint. To sum 
these velocities, they must be expressed in the same coordinate sys- 
tem. This summation requires the use of the transformation matrices 
from the previous section. Expressing everything in terms of the world 
axes, we have: 

(Me = 0 P 1 1^1 + 0 P 2 2—2 + + 0 P 7 7^7 

This single-vector equation yields a system of three linear scalar 
equations. 


To solve a system requires as many equations as the system has un- 
knowns. Consider any one of the joint angular velocity vectors w^. 
Expressing this vector in terms of the system Xi, we have 


i2i = 


i w ix 


i w iy 


i w iz 


and so it appears that each w-^ has three unknowns. 


However, the joints of the SOS manipulator are single degree of freedom 
joints. Therefore, all rotations take place around a single axis. By 
determining the relationship of the system Xj_ to the joint axis, the 
number of unknowns is reduced to one. In general, one of the }£i axes 
will coincide with the axis of rotation, so that equation (D3-3) will 
be of the form 


.w . 

1 XX 


0 


’ 0 

0 

or b) ,w. = 

.w. 

or c) .w. = 

0 


x— X 

x xy 

x~x 


0 




. w . 



0 


1 xz 


depending on which axis of X^ is in alignment with the joint axis. 

Thus, the system of equations (D3-2) in actuality contains at most 
seven unknowns. To obtain the remaining control equations, and to com- 
pletely determine end-effector motion, consider end-effector transla- 
tional velocity. 

Translational velocity can be represented as a three-vector, the com- 
ponents of which represent the magnitude of the velocities along the x> 
2, and z axes. Let v e denote end-effector translational velocity. 

Then ^ becomes 
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'ex 


'ey 


'ez 


To see how rotational joints can be used to produce translational mo- 
tion, consider the rotating bar (Fig. D3-1). 



Figure D3-1 Rotating Bar 


The bar is rotating with an angular velocity of w, and is r units 
long. However, at any instant of time, the motion is translational, 
along a line tangent to the curved path. The velocity at any given 
point is proportional to its distance from the center of rotation. A 
formula from mechanics gives the exact relationship as 

D3-5 v = w X r + vq 

That is, at any instant, translational velocity is equal to the vector 
cross product of angular velocity and the radius vector, plus the ve- 
locity of the origin. 

Thus, the rotational motion at each joint produces an instantaneous 
translational motion of the connecting joint. Let r^ denote the 
radius vector from the i joint to the origin of _X e . Then x 
is the vector cross product or translational velocity of the end 
effector that is produced by the angular velocity vector of joint i. 
Therefore, summing all such components yields the total translational 
velocity of the end effector: 

D3-6 0 Y e = 0 Pi (iHi x !£!) + . . • + o p 7 (7^7 x 7£7> 

Note that equation (D3-6) applies only to arms that have no sliding 
joints . 

This single vector equation yields three linear, scalar equations. 
Because the matrices jP^ and the vectors r^ are functions of the 
joint positions, the only unknowns are the single nonzero components of 
the vectors w^. 


D-10 



Combining equations (D3-2) and (D3-6) into a single equation: 


D3-7 


0 1 ^1-1 X 1-P + ••• + 0 P 7 X 7 - 7 ^ 

0 P 1 1^1 + ••• + o P 7 7-7 


The vector equation above yields six linear, scalar equations in seven 
unknowns. These are not enough equations to completely solve the sys- 
tem. There are ways of arriving at an answer by introducing another 
constraint (e.g., minimum energy expenditures), hence yielding a 
seventh equation. However, the configuration of the SOS manipulator 
offers another alternative. 


Examining the drawing of the SOS arm (Fig. D3-2) reveals that shoulder 
joints control elbow-position orientation. It is possible to select 
one of the joints to be controlled separately. In this case, the value 
of Wj_ for the shoulder joint selected is an input, rather than an un- 
known, and can, therefore, be eliminated from the control equations. 
This leaves six equations in six unknowns, so the system can be solved 
uniquely. 



Figure D3-2 The SOS Manipulator 
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For the sake of notation, let joint 1 be selected to be the joint con- 
trolled directly by the operator. Then the control equation (D3-7) 
takes the form: 
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0-e 


0“e 


0 P 2 ^2-2 x 2-2^ + • • • + o P 7 t ' 7 M 7 x 7-7^ 
0 P 2 2*2 + ' • ' + 0 P 7 7-7 


Evaluating the cross products and performing the internal matrix multi- 
plications, a matrix equation is obtained of the form: 
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0-e 


2 W 2j 


— 

j : 

~W 


• 

0-e 


_w^. 

i 


7 7k 


where w n j is the angular velocity of joint n, and j represents the 
axis of rotation (because joints have one degree of freedom, the rota- 
tion will be around a single-coordinate axis). In the case that joint 
n revolves around the x-axis, w n y = w nz = 0, therefore w^ is the 
unknown that must be calculated. 

Once the system of equations is in world coordinates, it is necessary 
to find only the matrix inverse (or pseudo-inverse) of J to solve the 
system. 

First, evaluate terms of the form Q p i x i£i) : 



" . w . 

1 IX 


r . 

X XX 

D-3-10 (.w. X .r.) = 

i“i i~i 

. w . 
i xy 

X 

. r . 
x xy 


.w . 

X iz 


,r . 
x xz 


1 i 

j 

k 


.w . 

X XX 

.w. 
x xy 

. w . 

X xz 

.r . 

X XX 

. r . 
x xy 

.r . 
x xz 


. w . 
x xy 

. r . - 

x xz 

.w . 

X xz 

. r . 
x xy 

. w . 

X xz 

.r . 

X XX 

.w . 

X XX 

. r . 
x xz 

.w. 

X XX 

.r . 
x xy 

•H 

.r . 

X XX 
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Oi P U 

p 

01 12 

0i P 13 


>> 

•H 

i r iz 

. w . 

1 XZ 

.r . 
i ly 

D-3-11 0 P i <i2 ± X ±Zi) = 

0i P 21 

0i P 22 

0i P 23 


.w. 

1 1Z 

i r ix " 

.w. 

1 IX 

± r iz 


0i P 31 

p 

Oi 32 

p 

Oi 33 


.w. 

1 IX 

i r iy _ 

.w. 

1 xy 

1 

X 

•H 

U 

•H 


0i P 13 

i r iy ■ 

p 

Oi 12 

i r iz> 

.w. 

1 IX 

0i P 23 

i r ±y ■ 

0i P 22 

i r iz ) 

.w . 

1 IX 

0i P 33 

i r iy " 

p 

Oi 32 

i r iz } 

.w. 

X XX 


+ 

( 0i p n 

i r iz 

0i P 13 

. r , ) -W, 

l ix l iy 

+ 

( 0i p 21 

i r iz 

" 0i P 23 

.r . ) .w. 
l ix'x iy 

+ 

( 0i P 31 

i r lz 

p 

Oi 33 

.r . ) .w. 
x ix x xy 


+ 

( 0i P 12 

. r . 

X XX 

p 

Oi 11 

. r . ) ,w. 
x xy x xz 

+ 

( 0i P 22 

.r . 

X XX 

p 

Oi 21 

. r . ) .w. 
x xy x xz 

+ 

( 0i P 32 

.r , 

X XX 

p 

Oi 31 

.r . ) .w. 
x xy x xz 


In actual practice, the vector gPi (j_w-£ x ^Xi) will have 
three terms, instead of nine. The reason is that the ith joint will 
have only one degree of freedom. Hence, two of the factors w.£ x , 
wj_y, and w.£ z will be zero, eliminating six terms from equation 
(D3-11) above. 
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Now evaluate terms of the form o?i iSi : 



p 

0i 11 

p 

Oi 12 

P 

0i 13 


.w. 

1 IX 



H* 

11 

0i P 21 

0i P 22 

0i P 23 


.w. 

1 ly 




P 

Oi 31 

p 

Oi 32 

P 

Oi 33 


.w. 

1 1Z 




0i P l 1 

i"i x + 

p 

Oi 12 

.w. + 

l ly 

p 

0i 13 

.w. 

1 1Z 

= 

0i P 21 

i w ix + 

0i P 22 

.w. + 

l ly 

0i P 23 

.w. 

X 1Z 


0i P 31 

i”ix + 

0i P 32 

.w. + 

l ly 

0i P 33 

.w. 

1 1Z 


Again, since two of the components £W£ X , ±w±y, and j_w iz 
will be zero, the vector qP ^ will have only three terms • 


So, the 

final 

form 

of J is: 






02 P lk 

2 r 2j 

“ 02 P lj 

2 r 2k 

07 P lk 7 r 7j 

p 

07 r l j 

7 r 7k 

D-3-13 Jw = 

02 P 2k 

2 r 2j 

p 

02 zj 

2 r 2k 

*'• 07 P 2k 7 r 7j 

p 

07 r 2j 

7 r 7k 


02 P 3k 

2 r 2j 

_ 02 P 3j 

2 r 2k 

•“ 07 P 3k 7 r 7j 

p 

07 r 3j 

7 r 7k 


p 

02 li 

p 

02 2i 

p 

02 3i 


p 

07 li 


p 

07 2i 

p 

07 3i 



2 w 2i 


3 W 3i 


4 w 4i 


5 W 5i 


6 W 6i 


7 W 7i 


— 


where, for each column, one of the following three cases holds: 

1) i - x , 1 2) i = z , 3 3) i = y,2 

j * y>2 j = x,l j = z , 3 

k = z , 3 k = y,2 k = x , 1 

Which of the three choices is to be used depends upon the joint axis of 
rotation. 

All that remains is determining the solution to the matrix rate 
equations . 
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4.0 


SOLVING MATRIX EQUATIONS 


A matrix equation with as many equations as unknowns has three possible 
classes of solutions: (1) one unique solution, (2) infinitely many 

solutions, and (3) no solution. If a unique solution exists, it can be 
found by inverting the matrix J. If many solutions exist, some criter- 
ia must be developed by which to select the best solution. If no solu- 
tion exists, then the closest approximation to a solution must be found. 

One technique of matrix inversion is the simple procedure generally 
taught in general math courses — Gaussian elimination. The procedure 
for inverting a nonsingular matrix M is as follows: 

1) Form the augmented matrix (M ; I) = A; 

2) Use elementary row operations to change M into the identity 
matrix. These operations are 

a) Interchange the position of rows, 

b) Multiply a row by a constant, 

c) Add a row times a constant to another row. 

These steps yield an augmented matrix of the form B = (I • M'), instead 
of A = (M - I). It can be shown that M“^ = M* . 

In general, a linear system Mx = k is solved by finding M“^ , then x = 
M”^b. However, using Gaussian elimination, it is not necessary to 
explicitly find • 

It can be shown that the performance of elementary row operations is 
equivalent to multiplication by a matrix: 

(M * I) — ►-row operations- — *-(I • M~l) . 

is equivalent to: 

(M • I) — ►PCM I) = (PM : PI) = (I • M -1 ) 

• • • • 

Furthermore, it can be shown that P = M“^. Therefore, 

P (M • b) = (PM ’• Pb) = (I • M - 1 b) = (I ; x) 

This means that the solution x = M~^b can be found by row-reducing 
the matrix (M • b) to the form (I ! x) . 


4.1 THE PSEUDO-INVERSE 


Not all matrix equations have unique solutions. It is possible for a 
system to have no solution, or for the system to have an infinite num- 
ber of solutions. As might be expected, each of the above alternatives 
leads to a different approach to solving the system. 
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If a system of equations has no solution, then a best or closest system 
solution must be estimated. Of course, the best possible estimate is 
desired. One criterion for a best estimate is minimal difference be- 
tween estimated and true output. That is, if Ax = b, is the original 
system (x is unknown) and x 1 is an approximation with Ax’ = b/ , then we 
seek to minimize |b-b ! | . We call the approximation that produces the 
minimum difference the ’’least squares" solution, or "least error" 
solution. 

The situation that has no solution often arises in "overdetermined" 
systems, in which there are more equations than unknowns describing the 
system. 

If A is of full rank — i.e., if all the columns of A are linearly inde- 
pendent — then the least-squares solution, x, of Ax = b is given by 

DA-1 x = (A t A)“^ A t b 

If a system of equations has an infinite number of solutions, then some 
criterion must be derived to select a solution. If the criterion is 
that of minimal vector length in the Euclidean sense, we call the solu- 
tion selected the "least effort" solution. That is, if (x]_, X2> 

X 3 , •••) all satisfy Ax = b, then the least effort solution is x where 

D4-2 x = min |x-j | 

j 

The situation of infinite solutions often arises in ’’underdetermined" 
systems in which there are fewer equations than unknowns. In this 
case, there are not enough constraints to completely determine the sys- 
tem. The underdetermined system can be represented by Ax = b where A 
is n x k; x is k x 1 ; b is n x 1; and n < k. If A is of full rank, 
then the solution of least length, x, or the least effort solution, is 
given by 

D4-3 x = A t (AA t )“^ b 

The pseudo-inverse is a matrix that combines all the above cases — as 
well as that of the unique solution — into a single matrix formula. Let 
A + denote the pseudo-inverse of a matrix A. Then, for Ax = b^, 

1) A + = A“1 if A is square and nonsingular; 

2) A + = (A t A)”-*- A t if A is n x k of full rank (n > k) ; 

3) A + = A t (AA t )”^- if A is n x k of full rank (n < k) . 

In addition, if A is not of full rank, then A + provides the least-er- 
ror solution of least length. 

The pseudo-inverse can be found by row-reducing the matrix A (Gaussian 
elimination) to a reduced row-echelon form. 

If A is n x k of rank r, this reduction proceeds to the point where. 


D-15 



1) The first r rows of A are nonzero; 

2) The last n - r rows of A are identically zero; 


3) The first r linearly independent 
umns of the identity matrix. 
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columns of A are the first r col- 


are reduced row-echelon forms; 


are not reduced row-echelon 
forms. 


Given a reduced row-echelon form of A, then A can be written as A = CD, 
where C is the n x r matrix consisting of the first r linearly inde- 
pendent columns of A (these are in the same positions as the columns of 
the identity matrix in the reduced row-echelon form), and D is the r x 
k matrix consisting of the non-zero rows of the row-echelon form of A. 
The equation A = CD is called a rank factorization of A. 

If A = CD is a rank factorization, then it is easy to find A + : 

D4-4 A + = D c (DD 1 - ) -1 ((^C) -1 C 11 
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A procedure for solving the matrix problem 


0-e 

= J 

2^2j 

0“e 


7™7l 


can now be outlined. 


First, formulate the problem as if J were nonsingular and proceed with 
row-reduction to find the inverse. If row-reduction produces one or 
more rows of zeroes, then J is singular, and the pseudo-inverse must be 
found. Use the row-reduced submatrix K to find a rank factorization of 
K. This, in turn, is used to find the pseudo-inverse of K, which can 
be used to solve the system. 

The pseudo-inverse of K may be used instead of the pseudo-inverse of J, 
provided the row-reduction matrix operations are used on F Xj • Con- 
sider the system w 

D4-6 Ax = b 
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where A and b are known. Solve for x. This is completely analogous to 
the original equation (D3-9). 

Form the augmented matrix and proceed with Gaussian elimination: 

D4-7 [A j I] 

If A is singular, row-reduce matrix A as far as possible to obtain 
matrix K. The augmented matrix becomes 

D4-8 [K • E] 

where E is the matrix operator that transforms A to K: 

D4-9 E[A • I] = [EA : El] = [K ’ E] 

• • • 

Solving the system using the pseudo-inverse of K is equivalent to solv- 
ing the original system if we replace b by Eb: 

D4-10 1 = K + Eb = (EA) + Eb = A + E + Eb = A + b, since E + E = I • 


SO 

D4-11 x = A~*"b , or "x = K^Eb, 

Using the pseudo-inverse of K is easier computationally because the 
formula for K + reduces to 

D4-12 K + = G t (GG t )" 1 


where K = FG is a rank factorization of K. 

To see this, if K is a row-reduced matrix, then the first r linearly 
independent columns are only the first r columns of the identity ma- 
trix. Hence, 

D4-13 F = Til J n rows 

|_ 0 J n-r rows 

and 


D4-14 (F t F) 1 
D4-15 Thus, 



(I • I + 0 • 0) -1 = I 1 = I 


(F t F) -1 F t = IFt = Ft 


which yields the simplified formula for K + . 

A flow diagram for the problem is presented in Figure D4-1. 
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Operator Inputs 



Joint Rate Commands 


Figure D4-1 Joint Rate Algorithm Flow Diagram 
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For a more detailed description of the development and the properties 
of the pseudo-inverse, see Subsection D7 of this report. 
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5.0 


TIMING 


The solution obtained for the matrix problem is only a instantaneous 
solution. That is, it remains correct only in a finite interval of 
time or distance from the position and instant in which it was calcu- 
lated. This is true for several reasons: 


1) Because the joints of the SOS manipulator are rotational, the 

end-effector motion will generally be only approximately linear. 
In fact, the end-effector path will usually be curved. 


2) The values used in computing the J matrix depend on joint posi- 
tions, which change as the manipulator moves. Therefore, the 
values of J (and hence J"^) are constantly changing. 


To produce accurate motion, the matrix problem must be solved repeated- 
ly. Frequently computed joint commands produce more accurate motion. 
Therefore, part of the problem in calculating the system is to solve it 
as rapidly as possible. This section briefly explores various tech- 
niques that can be used to speed up the solution of the matrix problem. 


The method originally employed for use in the SOS manipulator was to 
separate the translational and rotational control problems, and solve 
each using a different set of joints. 


The method implemented was to use the shoulder roll joint as separate 
operator input. The remaining three joints of the arm below the wrist 
were used to affect translational motion. Translational commands were 
input, producing a 3 x 3 control matrix that could easily be inverted 
to find the needed joint rates. 

The angular velocity of the end effector produced by these joints was 
then calculated and subtracted from the input angular velocity commands 
to obtain the net angular velocity. The three wrist joints were used 
to affect this net velocity, requiring the solution of a second 3x3 
matrix. 


The flow chart for this control system is depicted in Figure D5-1. 

Using this scheme, it is only necessary to invert small matrices, so 
that the entire solution process is accelerated. A problem with this 
technique is that while rotational effects of shoulder and elbow joint 
motion are considered in solving the rotation problem, the translation- 
al effects of wrist joint motion are not considered in solving the 
translation problem. 

A second option is similar to the first, but feeds all results back 
into succeeding calculations. Briefly stated, the translation commands 
are input, and any translation due to wrist joint motion is subtracted 
from them. This operation yields the net translation. The shoulder 
and elbow joints are used to produce the net translation, requiring the 
inversion of a 3 x 3 matrix. 
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0 W 1 x o r i 



Figure D5-1 Separate Translational and Rotational Control Flow 


The shoulder and elbow joint motions produce an end-effector rotation. 
This rotation is subtracted from the input commands to yield a net ro- 
tation. The wrist joints are used to effect the rotation, again re- 
quiring the inversion of .a 3 x 3 matrix. 

A flow diagram for this system is shown in Figure D5-2. 


0 W 1 x o r i 



5 W 5i 

6 W 6j 

7 W 7k 


Figure D5-2 Combined Translational and Rotational Control Flow 

This scheme accounts for the effects of each set of joints on both 
translation and rotation, but a problem may remain. It is not possible 
to exclude the case of an infinite loop in compensation. That is, it 
may be possible that- a rotation A0 may be necessary to compensate for a 
translation As, but to compensate for-A0, -As must be implemented, 
which means -A0 is necessary. This matter must be solved in some way 
before such a scheme is implemented. 
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6.0 


SUMMARY 


The problem considered in this document is that of interactive control 
of a robotic manipulator. The goal was to derive a method whereby man- 
ipulator end-effector motion can be directly controlled by human opera- 
tors without having to consider the motion of each individual joint. 

The solution was derived using the concept of coordinated rate con- 
trol. End-effector motions are input as rates; therefore, there are 
six inputs — three translational rates and three rotational rates, cor- 
responding to the three components of translational velocity and angu- 
lar velocity vectors. These end-effector rates are used to produce the 
necessary joint rates, thus effecting the desired motion. 

The transformation from end-effector rates to joint rates involves two 
concepts: 


1) The resultant velocity is the vector sum of the constituent 
velocities. 

2) The resultant angular velocity is the vector sum of the constituent 
angular velocities. 


These concepts yielded the two equations: 

6.1 0 - e = 0 P 2'2-2 X 2-2 ) + *** + 0 P 7 ( 7 2 7 X 7~7 ) 

and 

6.2 oH e = 0 P 2 2-2 + '•* + 0 P 7 7-7 


which were combined (after multiplying out terms) to yield the matrix 
equation 


foie" 

= J 

2 W 2j 

o 

IS 

(D 


w 

L J 


7 7k 


The matrix J may be singular, or nonsingular. This means that, given 
v e and w^ there may be one, more than one, or no vector 
2 w 2j» ’ • * •> 7 W 7 j c ] t that produces the specific output motion. 

Hence, a versatile way of solving the system was sought. This solution 
method was the pseudo-inverse, and an algorithm was outlined (and a 
flow chart drawn) for computing the pseudo-inverse of an arbitrary ma- 
trix J. 

Finally, timing considerations were investigated, yielding two methods 
of approximating the exact solution. Both methods require less calcu- 
lation time using the pseudo-inverse than the exact method. 
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7.0 


SOLUTION OF GENERAL LINEAR SYSTEMS 


This section sketches a theoretical development of the matrix pseudo- 
inverse, and is intended for the interested reader who is unfamiliar 
with the concept of "solving" singular linear systems. A complete 
theoretical development can be found in (Ref. 5)* The development pro- 
ceeds by analyzing the problems of obtaining best solutions to linear 
systems with more equations than unknowns, then systems with fewer 
equations than unknowns. The tools developed to solve these systems 
are then combined, in a natural way, to yield the pseudo-inverse, the 
tool used to solve general linear systems. 


7.1 OVERDETERMINED SYSTEMS 


Consider the matrix equation 
D7 .1-1 Ax = b 
where : 

A is n x k, known, k<n; 
b is n x 1, known; 
x is k x 1, unknown. 

Note that, in applying matrix multiplication, the first element of x 
multiplies every element in the first column of A. Similarly, the 
second element of x multiplies every element in the second column of 
A. In fact, if the i column of A is denoted A # i, then the equa- 
tion above can be rewritten 

D7.1-2 ^x^ ^ m 2 x 2 • • • A ^ ~ ' b 

This formulation shows that, for the system to have a solution, b must 
be a linear combination of the columns of A, or, b must be an element 
of CS(A), the column space of A. 

If b is not contained in CS(A), a solution x* is desired such that the 
minimum error occurs between the desired system state b and the system 
state b produced by "x. The Euclidean distance, |b-b|, must be mini- 
mized to produce the best estimate for x> denoted as x. The T) that 
minimizes |b-b| is the projection of b on the column space of A. 

Consider an element of an n-dimensional vector space V. CS(A), the 
column space of A, is k-dimensional, with k < n. The Gram-Schmidt 
theorem says that the basis of CS(A) can be extended using vectors or- 
thogonal to those in the basis of CS(A) to form a basis for the n-di- 
mensional vector space V. Therefore, V can be considered as the sum of 
two vector subspaces — CS(A) and CS(A) 1 (V = CS(A) + CS(A) f ). The 
vector b can now be written as the sum of two vectors, a contained in 
CS ( A) , and 3 contained in CS(A)*. 
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Since b =» a + g, let b^ be the projection of b into CS(A). 

Now, b-b^= a + 6 - 

So, |b - b^|2 = |a + 3 - b A |2 = |a - b A 1 2 + | fi |2 

because a and B are orthogonal. The vector b A is contained in CS(A), 
so changing b A can only affect the first term, which is minimized 
when a = b A ; i.e., when b^ is the component of b, in CS (A) . Now 
that it has been demonstrated that b A minimizes |b - b|, the original 
vector equation can be reformulated. 

Recall Ax = b, and Ax = b A 


where 


A .1 X 1 + A .2 x 2 + A .k x k - 


D7.1-3 


and A.^ + ... + A >k x k = b A 


D7.1-4 


D7.1-5 


The vector x consists of the components of b A in the basis of CS(A). 
The components of x* can be obtained by taking the inner product of fc 
with the basis of CS(A). Let i € jl, 2 , . . . ,k£ . Then for b = b A + b A ’ 

0 - Jv • A -i 

- (b - V • A. t 

= b • A . - b. • A . 

- • i —A • x 

therefore , 


D7.1-6 


-A ' 
but 


A . = b • A . 

•l - *i 


D7.1-7 b. = x,A , + x 0 A 0 + 
—A 1 • 1 2*2 

thus , 


+ x, A 1 = . $ x . A . 
k -k j = l j -j 


D7.1-8 


k - 


b • A . = (.z; x. A .) • A . 
-A • l V J = 1 J -x 


= . x . (A . • A . ) 
1=1 J -1 


Recall that, for any vectors a and B from the same space, 
D7.1-9 a • (3 = • a = 8 


D-24 



Thus, the dot product in D7.1-8 can be rewritten: 


D7 .1-10 b • A . = .^x.(A fc . A .) 

“A •! J=1 j' *x' 


= x,A t 1 A , + ...+ x, A t , A . 
1 *1 *i k *k 


or, in vector/matrix form: 

-A ’ A *i = f A -l A .i» •••» A ?k A .il 


D7.1-11 


?1 


X k 


Now, taking the dot product over all the vectors 

x. 


D7.1-12 


"A ' A .l 


b A • A . 
-A *k 


A . A _ . . . A - A - 

• 1 • 1 • 1 *k 


A t 1 A ! ... A* 1 - A . 

•k *1 *k *k 


x k 


= A C Ax 


Noting that • A = A ^ = A , equation D7.1-12 becomes 


D7.1-13A t Ax = 


“A * A .l 


.b . • A - 
L— A • k- 


= A C b 


The matrix A^ is k x k, made up of the inner product of k linearly 
independent vectors. It is therefore nonsingular. Equation D7.1-12 
can therefore be solved for x: 


D-7 .1-14 x = (A fc A) 1 A fc b 


The expression (A t A)“^A t is the general form of the matrix opera- 
tion used to obtain a least-squares solution of Ax = b, where A is n x 
k of rank k, k< n. 


7.2 UNDERDETERMINED SYSTEMS 

Consider the matrix equation 
D7.2-1 Ax = b 
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where 


A = n x k, known, full rank; 
x = k x 1, unknown; 
b = n x 1, known. 

If k > n, then the matrix equation contains fewer equations than un- 
knowns. Thus, fewer constraints exist to completely specify a unique 
solution, and thus the system is described as underdetermined. 

Due to the lack of sufficient constraints, the system has an infinite 
number of solutions. These solutions are said to be in a hyperplane, 
(also called an affine space), which is a vector subspace that has been 
displaced from the origin. 

Formally, a hyperplane can be defined by a vector added to every ele- 
ment of a subspace. That is, let U be a subspace, and £ be a vector 
that is not contained in U. Then _£ and U define a hyperplane 
H by : 

D7.2-2 H = q + U 

This definition is shown intuitively in Figure D7-1 by considering the 
two-dimensional case. A system with two unknowns but only one con- 
straint equation produces an infinite number of solutions, all of which 
lie on a line displaced from the origin. The line containing all the 
solutions is a one-dimensional hyperplane. 


x y + x - 1 



Figure D7-1 One-Dimensional Hyperplane 

One criterion for selecting a solution from the infinite set is to 
choose the vector in the set that has the shortest Euclidean length. 

By examining the two-dimensional case (above), the shortest vector in 
the set is seen to be the vector from the origin that is perpendicular 
to the line. 

In general, the solution sought in the underdetermined case is the vec- 
tor from the origin perpendicular to the hyperplane. This solution is 
referred to as the “least-effort" solution. 
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Consider the matrix equation (D7.2-1) in terms of linear combinations 
of columns of A. In this expanded form, the equation is written as 

D7.2-3 A <1 x 1 + ... + A . k x k = t 

Thus, b can be rewritten as the sum of n vectors. Because each of the 
k vectors is n-dimensional , n<k, the vectors k 9 ± are not linearly 
independent. (There can be a maximum of n vectors in a set of linearly 
independent vectors of dimension n. ) 

Therefore, there exists a vector y such that 

D7.2-4 A ,y 1 + . . . + A . y, = 0 
• r 7 1 • k J k 

Any vector jr such that Aj = 0 is a member of the null space of A [writ- 
ten as NS (A) ] . 

Let x be a solution vector of Ax = b. Now, if ^ I s contained in NS(A), 
then (x+y) is also a solution. This is proved as follows: 

A(x+y) = Ax + Ay 

07.2-5 = b + 0 

= b 

Every solution of Ax = b can be written as 
D7 . 2-6 x = x + x 

where x p is a solution of Ax “ b, and x n is an element of NS (A). 

Let H be the set of all solutions of Ax = b. Then, if x p is a given 

solution, 

D7.2-7 H = x + NS (A) 

“P 

That is, the solutions to the underdetermined system Ax = b lie in a 
hyperplane that is the null space of A translated from the origin. 

As discussed above, the least-effort solution to an underdetermined 
system is the shortest vector from the origin to the hyperplane. This 
shortest vector is intuitively seen (as above) to be perpendicular to 
the hyperplane. 

Given any solution, Xp> °f the equation Ax = b, the perpendicular can 
be computed. The perpendicular is the projection of x p onto the 
space perpendicular to NS(A). To see this, consider tne two-dimension- 
al case shown in Figure D7-2. 

The solution, x, is the sum of two vectors perpendicular to each 
other. The least-effort solution is therefore the vector component 
perpendicular to NS (A) that is contained in every solution. 

Let NS (A) ^represent the vector space perpendicular to NS (A). 
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Figure D7-2 

Two-Dimensional Under determined System 

Lemma: The columns of form a basis for NS(A)^ 

Proof: 

Let £n be in NS (A). Then, if x = 0, the vector £ is an element 
of NS (A) '". 

D7.2-8 Ax n = 0 

can be rewritten as 


D7.2-9 A <1 x 1 + ... + A #k x k = 0 


or 


A. - x- + . . . + A., x. =0 
11 1 lk k 


D7.2-10 I 


A x + . . . + A . X. =0 
nl 1 nk k 


but , 

0 = A il x l + + A ik x k = ^ A il’ A ik^ 


D7.2 11 = a. x 

x .— 

= (A,.)^ 


? II 


x . 

n 


So every column of A*- , when taken in an inner product with x> yields 
0. Therefore, every column of A fc lies in NS(A)^. Since there are 
n such columns and A is of full rank, all the n columns are linearly 
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independent. And, since the rank of NS(A) is k-n, the rank of NS(A)^ 
is n. So, the columns of A t are linearly independent and therefore 
form a basis for the n-dimensional space NS(A)**. 

The least-effort solution of equation (D7.2-1) can now be found. Let 
denote the least-effort solution. Then x is a linear combination of 
the columns at A^ : 

~ A t, A t ' , A t 

x = c.A t + c 0 A 0 + ... + c A 
- 1 *1 2 *2 n -n 

D 7. 2-12 = A t C 


Now 

D7 • 2-13 Ax = b 


or, 

D7.2-14 A( A t t) = b 


so 


D7.2-15 (AA t )£ = b 
and 

D7.2-16 c = (AA t ) _1 b 

But c is not the vector sought. The least-effort solution is £ = 
A t c. Multiplying both, sides of the equation (D7.2-16) by A t , yields 

D7.2-17 x = A fc c = A t (AA t )" 1 b 

The expression A t (AA t )“^- is the general form of the matrix opera- 
tor used to obtain the least effort solution of Ax = b 9 where A is 
n x k, n < k. 


7.3 GENERAL SYSTEMS 


The solution to general classes of matrix equations becomes the combi- 
nation of two solutions: (1) solutions to the overdetermined problem 

and (2) solutions to the underdetermined problem. 

Consider Ax = b 

where 

A is n x k, known; 
x is k x 1, unknown; 
b is n x 1, known. 
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For the general class of problems, n can be greater than k, or, vice 
versa, n can be less than k, or n can be equal to K. 

Assume A is of the rank r, r <. min(n,k) . Suppose A is rank-factored 
into 

7.3- 1 A = CD 

where each submatrix C, D is full rank. Then A = CD is called a 
"rank-factorization" of A. 

Then the equation to be solved may be rewritten as 

7.3- 2 CDx = b 


Let z = Dx; then Cz = b. 

C is n x r and full rank, so the system is overdetermined. The solu- 
tion to such an overdetermined system is given by 

D7.3-3 ~z = (C t C)“ 1 C t b 

However, ~z = Dx where D is r x k, r <. k, and full rank so that the sys- 
tem ~z = Dx underdetermined . The solution to an underdetermined system 
is given by 

D7.3-4 x = D c (DD 11 ) -1 !. 

The complete solution of the general matrix problem is the combination 
of equations (D7.3-3) and (D7.3-4). This solution is given by 

D7.3-5 § = D t (DD t ) _1 (C t C)" 1 C t b 

The pseudo-inverse of matrix A is denoted as A + : 

D7.3-6 A + = D t (DD t ) _1 (C t C) _1 C t 

It can be shown that A + yields the least-effort solution to the 
least-error problem. If A is nonsingular, it can be shown that A + = 

A" 1 . 

Lemma: A + = A~^ for A nonsingular. 

Proof : 

We need to show A + A = AA + = I. We will demonstrate only A + A = I, 
since the proof of the second part is virtually identical. 

A + A = D t (DD t ) _1 (C t C) _1 C t CD since A = CD 

= D t (DD t ) -1 (C t C) _1 (C t C)D 

= D t (DD t )° 1 D since (C t C) _1 (C t C) = I 

Because A is non-singular, and C, D are of full rank. (by definition 
of rank-factorization), then C, D must also be non-singular. Therefore 
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D t (D t )“ 1 D *D 


since (AB) ^ = B ^ 
for A,B non-singular 


I 
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APPENDIX E — SIMULATION METHODOLOGIES 


The analysis of any physical system must begin by characterizing the 
specific process to be modeled. This characterization must consider 
(1) the system type, (2) governing equations of the process, and (3) 
system questions to be answered. Table E-l shows these notions. 


Table E-l Process Characterization 

•Types of Systems 

- Deterministic, i.e., Noise-Free System 

- Stochastic, i.e., Systems with Noise 


•System Description 

- Ordinary Differential Equations 

- Partial Differential Equations 

- Algebraic Equations 
•System Questions to be Answered 

- Solution to Governing Equations 

- Estimation 

- Identification 

- Control 


Linear or 
Nonlinear 


Systems may be typed as being either deterministic or stochastic. A 
deterministic system is defined as one that incorporates no uncertainty 
and the stochastic type as one that includes uncertainty in the model. 
True systems may be represented by physical laws expressed by partial 
differential equations (PDE), ordinary differential equations (ODE), and 
algebraic equations (AE). Hence, the governing equations of the pro- 
cess must be given by PDE, ODE, AE, or a combination of the three. To 
answer the system questions, one must solve the governing equations. 

In the case of the deterministic type system, the solution may be easi- 
ly obtained; however, the stochastic system presents additional 
concerns. 

The solution of stochastic systems may be obtained by defining and 
solving the estimation, identification, and control (EIC) problems 
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associated with the given process. This study illustrates the need for 
solving the EIC problems as they pertain to simulation of a manipulator 
system. 


Open-loop methods (OLM) and closed-loop methods (CLM) are considered. 
OLMs are described generally by systems that lack feedback from the 
operation that is occurring. An OLM is presented in Figure E-l. 


Agreement 



Figure E-l Open-Loop Method 

The open-loop methodology in Figure E-l uses an a_ priori data base to 
provide predictions of outputs, which are compared with measured data 
from the test cell. Results can range from complete agreement to no 
agreement at all. In addition, the confidence level of results of a 
given test is typically low until a large statistical data base indi- 
cates the attributes of the samples. Typically, as one begins to ac- 
cumulate test data, the predictions are adjusted to accommodate these 
data. This "knob tweaking" is usually conducted by methods that are 
far from being mathematically rigorous. 

Because OLMs have considerable shortcomings, the need for a more sys- 
tematic approach has become evident in recent years. Modern, system 
theory provides such an approach based on CIMs that form the bases for 
most applications where feedback is used to control the system. The 
CIM is shown in Figure E-2 by using adaptive procedures that provide a 
model with quantified confidence levels. Modern system theory recog- 
nizes the potential differences in predictions and results that can be 
attributed to forcing function uncertainty, incorrect estimates of con- 
stituents in the governing equations, and the possibility that the mod- 
el order is insufficient to describe these systems. The adaptive pro- 
cessor design attempts to accommodate these uncertainties and to syste- 
matically drive the difference between predictions and test data to a 
minimum. 
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Disturbance System Equations 

Identification and Parameters 



Figure E-2 Closed-Loop Method 


The theoretical aspects required to obtain the systematic results of 
the CLM are introduced by presenting the thesis: "Control of the error 

that results from the comparison of data obtained from the true system 
to that produced by an a_ priori data base will result in: 

1) Obtaining a classification set, which implies the system can be 
modeled (this, in turn, implies that the system equations can be 
solved); 

2) Or denying the classification set, which implies that the system 
cannot be modeled." 

Acceptance of this thesis implies that the solution of the control 
problem is imperative to model the system. 

The term "classification set" has been introduced and needs further 
clarification. 


The notion of a set is generally defined in mathematical terms. In 
particular, this set, C, may be given as the set that contains the fol- 
lowing elements: independent variables, dependent variables, coeffi- 

cients of the governing equations, and the sensor output. Mathemati- 
cally this set can be written as: 


[E-l] 


c k 


INDEPENDENT 

VARIABLES 


DEPENDENT 

VARIABLES 


COEFFICIENTS 

OF 

EQUATIONS 


SENSOR 

OUTPUT 


Because this definition for the classification set holds for the propa- 
gation of a signal through any system, the elements of this set will be 
obtained from the equations that govern the evolution of the ROBSIM 
control signal. Equation [E-2] shows the relationship of estimation, 
identification, and control to the classification set. For example, 
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a solution to the estimation problem will yield estimates of the inde- 
pendent and dependent variables. Similarly, the solution to control 
encompasses the solution of both estimation and identification, in ad- 
dition to providing an estimate of sensor output.. 


[E-2] 


A I 


INDEPENDENT 

VARIABLES 

v 


ESTIMATION 


DEPENDENT 

VARIABLES 

/ 


COEFFICIENTS 

OF 

EQUATIONS 


SENSOR 

OUTPUT 


IDENTIFICATION 

"control " 


Equation [E-l] presents the classification set in terms of theory. 
Members of this set can also be defined in terns of simulation of a 
manipulator system in the following way: 


[E-3] 



REFERENCE 

INPUT 


CONTROL AND 
STATE 
VARIABLES 


V 

ESTIMATION 


DYNAMIC, 
CONTROL AND 
MEASUREMENT 
MODIFIERS 


UNCERTAINTIES 


V 




IDENTIFICATION 




CONTROL 


SENSOR 

OUTPUT 




where the corresponding EIC relationships are shown. 

Before further clarification of the classification set is feasible, the 
basic elements of the physical process must be considered. To do this 
for the OLM, a lower layer of Figure E-l must be produced. This second 
layer is shown in Figure E-3. From this figure, two basic elements may 
be defined as being (1) the subsystem under evaluation, and (2) a mea- 
surement device. The subsystem under investigation produces a measur- 
able set, M, and the measurement device acting on this set produces the 
output, Z. In addition to illustrating the basic elements, Figure E-3 
shows the basis for their physical makeup. 


ROBSIM Subsystems under Evaluation 

Measurable 
Set M k 

— N 

Measurement Device 

Basis for a priori Data Base 

- Statistical Information 

- Empirical Fits 

- Process Uncertainties 

- Physical Laws 

Basis of a priori Data Base 

- Transfer Functions 

- Measurement Uncertainties 

N 

1/ 


Signature 
Set, Z 


Test Cell/a priori Data Base 


Figure E-2 Second Layer of Physical Process 


E-4 





However, the description of the process does not allow total definition 
of the classification set. For example, the only elements of the 
classification set that are illustrated are the measurable set, M, and 
the signature set, Z. In addition, the measurable set contains only 
some of the dependent variables, none of the independent variable, and 
none of the coefficient set. 

This fact requires a further analysis of the layers to completely de- 
fine the notion of the classification set. Unfortunately, the third 
layer (Fig. E-4) is still not sufficient to completely identify this 
set. However, additional element members can be defined. Specifical- 
ly, some members of the observable set, 0, are distinguishable, as well 
as all of the independent variables (i.e., internal and external stim- 
uli). A further reduction is required to obtain the complete dependent 
variable and coefficient sets. This juncture requires a description of 
the specific problem of interest. Because this is the case, further 
clarification of the classification set will be postponed until the 
clarification can be applied to a specific example. 



Figure E-4 Third Layer Mechanization 


The definition of the system level problems involves terms such as 
state variable, control vector, and observations. For a definition of 
these terms, consider the fourth-layer schematic of Figure E-l as shown 
by Figure E-5. 






W 1 



Figure E-5 Fourth Layer a priori Model 

The figure represents a typical system in terms of its characteris- 
tics — independent and dependent variables. Dependent variables are 
defined as state variables and independent variables are designated as 
the control variables. 

The location of these quantities is internal to block 1 through n. In 
Figure E-5, the independent variables are designated by the p multi-in- 
puts, u. The dependent variables, x, are imbedded in the system compo- 
nents designated by blocks 1 through n. Definition of the system state 
vector would be incomplete without accounting for background uncertain- 
ties. The uncertainties are represented by the w terms illustrated in 
Figure E-5. Note that the measurable set (i.e., outputs of the system) 
generally does not consist of all dependent or state variables. 

The sensor system acts on the measurable set. This action takes place 
dynamically and statically. Specifically, these measurables enter into 
the sensing device and are subjected to phase and amplitude distortion 
(i.e., dynamics) and ultimately sensed as a voltage (i.e., static). 
Hence, dynamics of the sensor must be accounted for in addition to dy- 
namics of the system. In this sense, dynamics attributable to the sen- 
sor are typically augmented to the state vector with dimension n to 
produce a system state of dimension n+l . Because sensing devices have 
errors associated with them, the signature produced by the sensor will 
be corrupted by this uncertainty. This is designated by the quantity, 
v (Fig. E-5). Addition of the noise quantity produces a signature set 
designated by the term, Z. 

To solve the EIC problems associated with simulation of a manipulator 
system, it is imperative that the system be modeled by using a state- 
variable (SV) format. 
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The SV framework was motivated by: (1) solutions to the estimation, 

identification, and control problems that require a SV representation; 
(2) control mobility and data processing/extraction algorithms that can 
be derived from solutions to the EIC problems; (3) use of SV that do 
not limit design to the time domain (i.e., state variables can be ex- 
pressed in the frequency domain, thus allowing classical design). 

To illustrate this concept, consider Figure E-6, which represents a 
simplified block diagram of the simulation tool elements. 



Figure E-6 Simulation Tool Simplified Elements 

Because this effort is to mathematically model the manipulator system 
such as that depicted in Figure E-6, this system must be represented in 
a mathematical format. Figure E-7a represents simplified elements of 
the simulator tool in a mathematical context. Note that the system as 
configured will have discrete and continuous components. Because sys- 
tems of this type are difficult to analyze, it is convenient to discre- 
tize the entire system. Figure E-7b illustrates an equivalent discrete 
system. 


Quantization System 



Figure E-7 Simulation Tool Element Representations 
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To further justify the use of SV as the framework of the simulation, 
Figures E-8a and E-8b illustrate that design and analysis of the system 
are not limited to either the time or frequency domain. (These figures 
show the system equivalence in both domains.) 



Figure E-8 Equivalent Systems 


As a further justification of the SV framework, consider an example of 
the problem: Suppose that it is desirable to have the manipulator fol- 

low a predescribed reference trajectory given by r(l), r(2), . .., 
r(n). Assume that certain members of the classification set are 
known. Specifically they are the reference input, random uncertainty, 
all measurement modifiers, and certain members of the dynamic and con- 
trol modifiers. 

To solve the control problem it is imperative to eliminate the uncer- 
tainty of the system. To do this, estimates of the control and state 
variables are required. Having obtained these results, the identifica- 
tion problem can be solved. Solution of the identification problem 
will produce an optimal digital controller for the system. 

Consider the system given by Figure E-7b. The classification set for 
this configuration is given as 

DYNAMIC , 

CONTROL AND 

REFERENCE CONTROL AND MEASUREMENT SENSOR 

[E-4] . INPUT STATE VARIABLE MODIFIERS UNCERTAINTIES OUTPUT 



CONTROL 
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where the roles of estimation and identification are labeled. Given 
the set of assumptions, it should be noted that the identification 
problem is limited to solving uncertain parameters in <J> and 0' — spe- 
cifically those elements that pertain to the digital compensator. Fig- 
ure E-9 shows how this process is mechanized, and the use of the Kalman 
filter to produce estimates of the state vector, x, and propagation of 
the uncertainty in the estimate, o^; and describes how the performance 
measure, J, is used to update parameters in <f> and 0. 



Estimation and Identif ication-Klontrol 


Figure E-9 Control Mechanization 
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